回想起曾學習A-star尋徑算法時，難以透徹理解其原理和機制，但隨著對圖和搜索算法的理解愈發深入，近期重拾A-star時發現并沒有那么困難。因此對A-star算法和A-star變種算法進行系統地學習，同時對其在游戲開發中的應用做了更深層次上的了解。

首先需要感謝Amit's A star Page和A-star算法中譯本，讓我能夠全面地了解A-star算法，下面大部分內容也是由原文及中譯文中提煉而得。

1. 從Dijkstra算法和最佳優先搜索到A-star算法

1.1 Dijkstra算法

核心思想：

每次都選取距離起始點最近的點，并加入完成列表。算法的效率并不高，但對于沒有負數權值邊的情況能夠得到從起始點到大部分點的最短路徑。Dijkstra算法相當于啟發式函數h(x) = 0的A-star算法（在后面會詳細說明）。

適用領域：

相對于A-star算法，Dijkstra算法也有它適用的領域。當移動單位需要找到從其當前位置出發到N個分散目的地中的一個的最短路徑（比如盟軍只需搶占N個據點中的任意一個就能取勝，此時盟軍需要選擇一個離自己最近的據點i即可），Dijkstra算法會比A-star算法更為合適。

原因在于，Dijkstra對所有中間節點一視同仁，并不考慮它們到目的地的實際代價，這就導致該算法會偏執地選擇離起點最近的節點，而當當前擴展節點集中出現了任意一個目的地i，算法就可以終止，而得到的目的地i就是N個目的地中距起始點最近的目的地。相反地，A-star算法則需要找到從起始點出發到所有目的地的最短路徑，通過比較得到所有最短路徑中的最小值。而實際上我們并不需要知道除了最近的目的地之外的其他目的地的路徑就是如何。

總而言之，A-star算法更適用于單點對單點的尋徑；Dijkstra算法更適用于單點到多點的尋徑。

1.2 最佳優先搜索算法（Best-fit）

核心思想：

最佳優先搜索算法的思想恰恰與Dijkstra算法相反，它忽略了從起始點到當前節點所花費的實際代價，而偏執地選擇當前節點集中“它認為”距目的地最近的節點并進行拓展。之所以稱為“它認為”，那是因為當前節點并不知道它到目的地的距離，而是通過一個評價函數（或稱啟發式函數）來估計從當前位置到達目的地的距離。由此可知，評價函數的選擇對結果和效率會有較大的影響。

適用領域：

不可否認，最佳優先搜索算法拓展的節點具有明顯的方向性（沒有障礙時方向會始終指向目的地），從而忽略了很多遠離目的地的節點，只有當迫不得已時，才會去拓展那些節點。這樣優秀的品質使得Best-fit算法的效率比Dijkstra算法要高很多。但速度快也是要付出相應代價的，很可惜Best-fit算法很多時候并不能找到最短路徑。同時，最佳優先搜索算法一般也只適用于單點對單點的尋徑。

1.3 A-star算法

核心思想：

A-star算法很巧妙地結合了Dijkstra算法和Best-fit算法的優點，一方面通過設定代價函數g(x****)來考慮從起始點到當前點的實際代價，另一方面通過設定啟發式函數h(x****)來考慮從當前點到目的地的估計代價，f(x) = g(x) + h(x)。它是從當前點擊中取出f值最小的節點并進行拓展。因此A-star算法具備了Best-fit算法的效率，同時又兼顧Dijkstra算法的最短尋徑能力。

適用范圍：

適用于單點對單點的尋徑。而且可以得到最短路徑，效率較高。

2 A-star算法的核心思想

2.1 代價函數和啟發式函數的權衡

• 代價函數g：g代表從起始點到當前節點的實際代價，每次拓展時將當前節點的g值加上從當前節點走向拓展節點所花費的實際代價，就是拓展節點的實際代價（從起始點到拓展節點）。
• 啟發式函數h：h代表從當前節點到目的地的估計代價，可以采用曼哈頓距離、對角線距離、歐幾里得距離來估計，但一般不建議使用歐幾里得距離（因為歐幾里得距離比另外兩種距離要小，雖然仍可以得到最短路徑，但計算的時間開銷更大）。
• g和h的權衡：代價函數g和啟發式函數h的相對關系會影響到效率。當g比h占據更多權重時，A-star算法更貼近于Dijkstra算法，算法效率降低。當g比h占據更少權重時，A-star算法更貼近于Best-fit算法，算法效率提高，但可能在尋找最優解的路上走得更加曲折。此外應注意，只有當啟發式函數h估計的代價小于等于真實代價時，才一定能得到最短路徑；否則，必須在代碼中做一些修改才可以得到最短路徑（后面詳細說明）。

2.2 代價函數和啟發式函數的修正

上一節已經說明g和h的相對關系會影響效率和結果。因此也發展出了一系列調整代價函數和啟發式函數的方法。權衡和修正代價函數和啟發式函數是一個很tricky的過程，當我們增加g的權重，那么我們會得到最短路徑但計算速度變慢；如果我們增加h則可能放棄了最短路徑，同時使得A-star算法更快。

但反觀我們使用A-star算法的初衷，我們只是希望得到一個較好的路徑使得游戲玩家到達目的地，而且游戲玩家通過主觀也無法精確判定他所走的路徑是否是最佳路徑。因此我們可以在修正中略微增加啟發式函數h的比重，從而使得A-star算法更快。

但應注意，g和h的衡量單位必須一致，其中任意一個值過分的大都有可能造成A-star退化為Best-fit或Dijkstra算法。

• a) g'(n) = 1 + α * (g(n) - 1)：
  代價函數g可以是1到實際g(n)間的一個數，這個數取決于你對效率的要求。
  當α=0時，g'(n)=1，相當于
  但不能過分的小，否則會退化為Best-fit算法。
• b) f(n) = g(n) + w(n) * h(n)：
  w(n)≥1且隨著當前點接近目的地而減小。設置這個系數的目的：在游戲尋徑過程中，響應速度較為重要。因此在尋徑初期增加w(n)值能夠使其尋徑速度增加，而且此時的尋徑準確度實際上并不重要。而當接近目的地時，減小w(n)使得尋徑更為準確。
• c) h(n) = h(n) * (1 + l/p)：
  其中，p為所能接受的最大路徑長度，l為移動一步的最小代價。
• d) 向量叉積修正法：
  • ①我們首先構造兩個向量：第一個向量是從起始點->目的地的向量vector1(dx1, dy1)，第二個向量是從當前點->目的地的向量vector2(dx2, dy2)。
  • ②現在我們對vector1和vector2求叉積：crossProduct = vector1 x vector2 = |dx1dy2 - dx2dy1|
  • ③h += crossProduct * 0.001
    crossProduct實際上度量了路徑偏離vector1的程度，也就是說這個調整使得算法更傾向于選擇靠近起始點到目的地連線上的路徑。
    印象中，上次使用叉積是在凸包算法中使用叉積來獲取2個向量間左偏或右偏的關系。
• e) 導航點法：
  對于地圖障礙較多的情況，啟發式函數h會過分地低估從當前點到目的地的實際代價，因此算法的計算速度會受到一定的影響。此時如果在圖中事先增加一些導航點（waypoints），而這些導航點間的實際代價已經事先計算好。那么我們的啟發式函數可以改寫成：
  h(curr, dest) = h'(curr, wp1) + distance(wp1, wp2) + h'(wp2, dest)
  上式中h'(A, B)是估計A到B的代價的評價函數，可以采用曼哈頓距離等。wp1和wp2分別是距當前點和目的地最近的導航點。由此我們使得從當前點到目的地的實際代價不會被過分低估，從而提高了計算的速度。

3 A-star算法的數據結構實現

下面是拷貝自Amit's A star Page原本的算法偽代碼：

OPEN = priority queue containing START
 CLOSED = empty set while lowest rank in OPEN is not the GOAL:
   current = remove lowest rank item from OPEN
   add current to CLOSED
   for neighbors of current:
     cost = g(current) + movementcost(current, neighbor)
     if neighbor in OPEN and cost less than g(neighbor):
       remove neighbor from OPEN, because new path is better
     if neighbor in CLOSED and cost less than g(neighbor):
        remove neighbor from CLOSED
     if neighbor not in OPEN and neighbor not in CLOSED:
       set g(neighbor) to cost
       add neighbor to OPEN
       set priority queue rank to g(neighbor) + h(neighbor)
 reconstruct reverse path from goal to start
 by following parent pointers

核心思想：

• ①維護一個關閉列表和一個開放列表。關閉列表初始為空，開放列表初始包含起始點。
• ②取開放列表中f值最小的節點v。如v為目的地則退出循環，否則將節點v放入關閉列表（表示節點v暫時為最優）。
• ③對節點k的所有鄰接節點w：
  • i) w既不在開放列表也不在關閉列表：計算w的g、h和f值，并將節點w放入開放列表；
  • ii) w在開放列表：計算w的g并與開放列表中的舊節點w的g值比較，如新g小于舊g，則更新開放列表中的舊g值和舊f值；
  • iii) w在關閉列表：計算w的g并與關閉列表中的舊節點w的g值比較，如新g小于舊g，則將w從關閉列表中取出并放入開放列表，并使用新的g、h和f值；
  • ④返回步驟②；

注意：

• 步驟③中iii)并不一定是必要的。當啟發式函數h(n)沒有高估從當前節點到目的地的代價，iii的情況是不會發生的。但在游戲中移動單位尋徑時，為了加快計算速度，h函數高估代價是很有可能發生的。

3.1 數組或鏈表

普通的數組或鏈表由于沒有次序，數組掃描、查找最小f值節點、刪除都需要O(n)，插入和更新為O(1)；鏈表掃描、查找最小f值節點需要O(n)，插入、刪除和更新為O(1)。除此之外，還有諸如已排序數組、已排序鏈表、排序跳表等。但都不是適用于A-star算法的數據結構，此處不再深究。

3.2 索引數組

索引數組是將所有網格按次序編號，各數組下標下存儲對應的各類數值。索引數組的掃描和查找最小f值節點為O(n)，插入、更新、刪除為O(1)。較好的插入和刪除性能使得它能夠完成一部分的操作。但索引數組只能用在那些網格較少的A-star算法中，對于網格數量太過龐大的情況，實際上被使用的網格只占所有網格中的一小部分。造成存儲空間的浪費。

3.3 散列表

散列表是索引數組的改進版本。索引數組作為無沖突的哈希表會造成存儲空間的浪費。散列表需要解決沖突，hash函數和沖突的處理方法會影響到散列表的性能。

3.4 堆

堆無疑是比較適用于A-star算法的一種數據結構。且在C++的STL中有二叉堆的實現（priority_queue）。堆查找最小f值節點為O(1)，刪除或調整為O(logn)，插入為O(logn)，掃描為O(n)。

3.5 伸展樹

伸展樹Splay使用了90-10原則，一般認為龐大的數據中經常被使用的數據只占總數據的10%。伸展樹查找最小f值節點為O(logn)，刪除為O(logn)，插入為O(logn)，掃描+調整為O(logn)。各方面性能均衡。但縱觀Splay樹的實現過程可以發現，伸展樹的核心操作Splay每次將需要操作的節點提升至根節點。這就意味著，每次查找和刪除最小f值節點都需要將節點從伸展樹的底部搬運到樹根，這樣的做法似乎很麻煩，并沒有很好地契合每次取最小值點的這個要求。從這一點來看，它并沒有比堆更優秀。

3.6 HOT隊列

在看這篇文檔前，我并未深入學習過HOT隊列（Heap on Top），最多只是知道名字和原理。在文檔中提出，HOT隊列是比堆更優秀，更適用于A-star算法的數據結構。它用桶來取代普通二叉堆中的節點，每個桶中有若干元素。但只有堆頂桶內的節點們按照堆建立關系，而其余的桶內都是無序的節點。這一點非常契合A-star算法的要求，因為拓展的一部分節點由于f值較大實際上根本不會被使用，那就可以讓它呆在下面的桶中，不參與堆化。
考察A-star算法，實際上每次取出的總是f值最小的節點，而插入的節點的f'值只可能是f ≤ f' ≤ f + df。此處df是一步移動造成代價增加的最大值。這樣我們就可以利用df來劃分各個桶，堆頂的桶使用的頻率最高。當堆頂桶空時，將下面的桶提升上來，并使其堆化，這個操作叫做heapify。假設共有k個桶，這就使得HOT隊列的查找為O(1)。對堆頂桶，插入和刪除為O(log(n/k))；對其他桶，插入為O(1)。heapify操作為O(n/k)。

3.7 數據結構的選擇和實現

對網格數不多的情況，一方面使用索引數組確保O(1)檢查是否需要調整或確定節點是否在開放/關閉列表中；另一方面使用堆（或HOT隊列）來實現對數時間復雜度的插入和刪除操作。

4 游戲開發中的應用

游戲中的尋徑相比于理想化的尋徑要復雜得多。需要考慮各方面因素，如區域搜索、群運動、響應和延遲、移動障礙物、位置或方向信息的存儲等。針對不同的游戲特點，需要不同的算法策略。

4.1 區域搜索

只需規劃從起始點到區域中心的路徑。當從開放列表中取出任意一個區域內的節點時，就認為已經到達該區域。

4.2 群運動

游戲中有時會讓多個游戲單位移動到同一處目的地，比如魔獸爭霸這類即時戰略游戲。如果為群中的每一個游戲單位使用A-star算法單獨規劃路徑，那這會使時間開銷成倍的增長，而且游戲單位的路徑會有明顯的重疊或平行的現象。

• a) 使用A-star算法規劃從群體中心到目的地中心的路徑，所有移動單位共享大部分的路徑。比如移動單位自己規劃前N步的移動路徑，此后使用共享路徑。接近目的地時，移動單位結束使用共享路徑，轉而使用自己規劃的移動路徑。在移動過程中可以增加一些擾動，讓各個單位的移動方向看起來不是那么相同。
• b) 在群眾設定一個領導個體，只為領導個體規劃路徑，而其余個體采用flocking算法以組的形式運動。

4.3 響應與延遲

一般游戲中我們關心的是響應時間。也就是說我們不希望看到，當按下導航按鍵后，游戲單位卻遲遲沒有開始移動。因此我們需要在響應時間（計算速度）和路徑優化之間做一個權衡。那我們如何在確保路徑最優化的同時保證較短的響應時間呢？有以下幾個辦法：

• a) 在路徑搜索初期的幾次循環中，可以快速地確定先前的N步移動。此時我們可以讓游戲單位先動起來，即使他們走的并不是真正的最短路徑。這樣在初始的幾步移動中就為A-star算法計算后續路徑提供了較為充裕的計算時間。有時，我們甚至可以讓游戲單位在一開始就向著起始點->目的地的直線運動，直到計算出路徑。
• b) 地圖地形復雜會使得A-star算法的計算時間變長。所以對于由地形（如道路、草地、山地，不同地形的移動代價不同）引起的計算時間延長。這樣我們可以適當減小地形的影響，比如山地移動的代價為3、草地為2、道路為1，就可以變更為山地1.2、草地1.1、道路1，使A-star算法先快速計算出一條比較合理的路徑讓游戲單位先移動起來。此后再按照設定的代價進行精確計算。
• c) 在空曠的野外場地，適當增大網格的劃分，可以有效縮短計算時間。當靠近目的地或復雜的城市地形時，減小網格劃分，使路徑更為合理。
• d) 設置導航點。在游戲地圖上事先布置好導航點，在導航點之間的路徑已經預先規劃并保存。因此尋徑時只需規劃從起始點到最近的導航點wp1，從目的地到最近的導航點wp2之間的路徑。整個路徑就被劃分為start->wp1->wp2->dest，而wp1到wp2是已有的。

4.4 其他算法

這里就不仔細介紹了。比如雙向搜索、帶寬搜索、Potential Field算法等。雙向搜索能并行計算，加快尋徑速度，當兩頭相連時尋徑成功；帶寬搜索能夠在用戶可忍受的路徑代價范圍內按照一定策略舍棄一些較差的節點，加快計算速度；Potential Field作為一個有趣的算法，是參考了勢能場和電荷運動的規律。適合場地較為寬闊的游戲地圖，而且該算法能保證不重疊。

4.5 移動障礙物

• a) 當游戲單位附近的世界改變時，或游戲單位到達一個導航點時，或每隔一定步數后，重新計算路徑。
• b) 在發現規劃路徑上走不通后，比如A->B的路徑被阻擋。重新規劃從A->B的路徑。B之后的部分仍保留原來的路徑。
• c) 對于多個游戲單位存在的情況。當物體接近時，改變其中一部分物體在已規劃好的路徑上的移動代價，從而讓一些移動單位避開其他單位。

4.6 位置和方向的存儲

路徑的存儲有時候是必須的。那位置和方向我們應該選擇存儲哪一個？顯然位置的存儲會消耗更多的空間。但存儲位置的優點在于我們可以快速查詢路徑上任意的位置和方向，不需要沿著路徑移動。

路徑壓縮：

• a) 如果存儲方向，我們可以用這種存儲的方式。如(UP, 5), (LEFT, 3), (DOWN, 3), (RIGHT, 4)表示上移5，左移3，下移3，右移4。
• b) 如果存儲位置，那么可以存儲每一個轉折點，轉折點之間使用直線相連。

//////////////////////////////////////////////////////////////////////
// Amit's Path-finding (A*) code.
//
// Copyright (C) 1999 Amit J. Patel
//
// Permission to use, copy, modify, distribute and sell this software
// and its documentation for any purpose is hereby granted without fee,
// provided that the above copyright notice appear in all copies and
// that both that copyright notice and this permission notice appear
// in supporting documentation.  Amit J. Patel makes no
// representations about the suitability of this software for any
// purpose.  It is provided "as is" without express or implied warranty.
//
//
// This code is not self-contained.  It compiles in the context of
// my game (SimBlob) and will need modification to work in another
// program.  I am providing it as a base to work from, and not as
// a finished library.
//
// The main items of interest in my code are:
// 
// 1. I'm using a hexagonal grid instead of a square grid.  Since A*
//    on a square grid works better with the "Manhattan" distance than with
//    straight-line distance, I wrote a "Manhattan" distance on a hexagonal
//    grid.  I also added in a penalty for paths that are not straight
//    lines.  This makes lines turn out straight in the simplest case (no
//    obstacles) without using a straight-line distance function (which can
//    make the path finder much slower).
//
//    To see the distance function, search for UnitMovement and look at
//    its 'dist' function.
//
// 2. The cost function is adjustable at run-time, allowing for a
//    sort of "slider" that varies from "Fast Path Finder" to "Good Path
//    Quality".  (My notes on A* have some ways in which you can use this.)
//
// 3. I'm using a data structure called a "heap" instead of an array
//    or linked list for my OPEN set.  Using lists or arrays, an
//    insert/delete combination takes time O(N); with heaps, an
//    insert/delete combination takes time O(log N).  When N (the number of
//    elements in OPEN) is large, this can be a big win.  However, heaps
//    by themselves are not good for one particular operation in A*.
//    The code here avoids that operation most of the time by using
//    a "Marking" array.  For more information about how this helps
//    avoid a potentially expensive operation, see my Implementation
//    Nodes in my notes on A*.
//
//  Thanks to Rob Rodrigues dos santos Jr for pointing out some
//  editing bugs in the version of the code I put up on the web.
//////////////////////////////////////////////////////////////////////

#include "Path.h"

// The mark array marks directions on the map.  The direction points
// to the spot that is the previous spot along the path.  By starting
// at the end, we can trace our way back to the start, and have a path.
// It also stores 'f' values for each space on the map.  These are used
// to determine whether something is in OPEN or not.  It stores 'g'
// values to determine whether costs need to be propagated down.
struct Marking
{
    HexDirection direction:4;    // !DirNone means OPEN || CLOSED
    int f:14;                    // >= 0 means OPEN
    int g:14;                    // >= 0 means OPEN || CLOSED
    Marking(): direction(DirNone), f(-1), g(-1) {}
};
static MapArray<Marking>& mark = *(new MapArray<Marking>(Marking()));

// Path_div is used to modify the heuristic.  The lower the number,
// the higher the heuristic value.  This gives us worse paths, but
// it finds them faster.  This is a variable instead of a constant
// so that I can adjust this dynamically, depending on how much CPU
// time I have.  The more CPU time there is, the better paths I should
// search for.
int path_div = 6;

struct Node
{
    HexCoord h;        // location on the map, in hex coordinates
    int gval;        // g in A* represents how far we've already gone
    int hval;        // h in A* represents an estimate of how far is left

    Node(): h(0,0), gval(0), hval(0) {}
    ~Node() {}
};

bool operator < ( const Node& a, const Node& b )
{
    // To compare two nodes, we compare the `f' value, which is the
    // sum of the g and h values.
    return (a.gval+a.hval) < (b.gval+b.hval);
}

bool operator == ( const Node& a, const Node& b )
{
    // Two nodes are equal if their components are equal
    return (a.h == b.h) && (a.gval == b.gval) && (a.hval == b.hval);
}

inline HexDirection ReverseDirection( HexDirection d )
{
    // With hexagons, I'm numbering the directions 0 = N, 1 = NE,
    // and so on (clockwise).  To flip the direction, I can just
    // add 3, mod 6.
    return HexDirection( ( 3+int(d) ) % 6 );
}

// greater<Node> is an STL thing to create a 'comparison' object out of
// the greater-than operator, and call it comp.
typedef vector<Node> Container;
greater<Node> comp;

// I'm using a priority queue implemented as a heap.  STL has some nice
// heap manipulation functions.  (Look at the source to `priority_queue'
// for details.)  I didn't use priority_queue because later on, I need
// to traverse the entire data structure to update certain elements; the
// abstraction layer on priority_queue wouldn't let me do that.
inline void get_first( Container& v, Node& n )
{
    n = v.front();
    pop_heap( v.begin(), v.end(), comp );
    v.pop_back();
}

// Here's the class that implements A*.  I take a map, two points
// (A and B), and then output the path in the `path' vector, when
// find_path is called.
template <class Heuristic>
struct AStar
{
    PathStats stats;
    Heuristic& heuristic;
    // Remember which nodes are in the OPEN set
    Container open;
    // Remember which nodes we visited, so that we can clear the mark array
    // at the end.  This is the 'CLOSED' set plus the 'OPEN' set.
    Container visited;
    Map& map;
    HexCoord A, B;
    
    AStar( Heuristic& h, Map& m, HexCoord a, HexCoord b )
        : heuristic(h), map(m), A(a), B(b) {}
    ~AStar();

    // Main function:
    void find_path( vector<HexCoord>& path );

    // Helpers:
    void propagate_down( Node H );
    Container::iterator find_in_open( HexCoord h );
    inline bool in_open( const HexCoord& h )
    {
        return mark.data[h.m][h.n].f != -1;
    }
};

template<class Heuristic>
AStar<Heuristic>::~AStar()
{
    // Erase the mark array, for all items in open or visited
    for( Container::iterator o = open.begin(); o != open.end(); ++o )
    {
        HexCoord h = (*o).h;
        mark.data[h.m][h.n].direction = DirNone;
        mark.data[h.m][h.n].f = -1;
        mark.data[h.m][h.n].g = -1;
    }
    for( Container::iterator v = visited.begin(); v != visited.end(); ++v )
    {
        HexCoord h = (*v).h;
        mark.data[h.m][h.n].direction = DirNone;
        mark.data[h.m][h.n].g = -1;
        assert( !in_open( h ) );
    }
}

template <class Heuristic>
Container::iterator AStar<Heuristic>::find_in_open( HexCoord hn )
{
    // Only search for this node if we know it's in the OPEN set
    if( Map::valid(hn) && in_open(hn) ) 
    {
        for( Container::iterator i = open.begin(); i != open.end(); ++i )
        {
            stats.nodes_searched++;
            if( (*i).h == hn )
                return i;
        }
    }
    return open.end();
}

// This is the 'propagate down' stage of the algorithm, which I'm not
// sure I did right.
template <class Heuristic>
void AStar<Heuristic>::propagate_down( Node H )
{
    // Keep track of the nodes that we still have to consider
    Container examine;
    examine.push_back(H);
    while( !examine.empty() )
    {
        // Remove one node from the list
        Node N = examine.back();
        examine.pop_back();

        // Examine its neighbors
        for( int dir = 0; dir < 6; ++dir )
        {
            HexDirection d = HexDirection(dir);
            HexCoord hn = Neighbor( N.h, d );
            if( in_open(hn) )
            {
                // This node is in OPEN                
                int new_g = N.gval + heuristic.kost( map, N.h, d, hn );

                // Compare this `g' to the stored `g' in the array
                if( new_g < mark.data[hn.m][hn.n].g )
                {
                    Container::iterator i = find_in_open( hn );
                    assert( i != open.end() );
                    assert( mark.data[hn.m][hn.n].g == (*i).gval );
                    
                    // Push this thing UP in the heap (only up allowed!)
                    (*i).gval = new_g;
                    push_heap( open.begin(), i+1, comp );

                    // Set its direction to the parent node
                    mark.data[hn.m][hn.n].g = new_g;
                    mark.data[hn.m][hn.n].f = new_g + (*i).hval;
                    mark.data[hn.m][hn.n].direction = ReverseDirection(d);
                        
                    // Now reset its parent 
                    examine.push_back( (*i) );
                }
                else
                {
                    // The new node is no better, so stop here
                }
            }
            else
            {
                // Either it's in closed, or it's not visited yet
            }
        }
    }
}

template <class Heuristic>
void AStar<Heuristic>::find_path( vector<HexCoord>& path )
{
    Node N;
    {
        // insert the original node
        N.h = A;
        N.gval = 0;
        N.hval = heuristic.dist(map,A,B);
        open.push_back(N);
        mark.data[A.m][A.n].f = N.gval+N.hval;
        mark.data[A.m][A.n].g = N.gval;
        stats.nodes_added++;
    }

    // * Things in OPEN are in the open container (which is a heap),
    //   and also their mark[...].f value is nonnegative.
    // * Things in CLOSED are in the visited container (which is unordered),
    //   and also their mark[...].direction value is not DirNone.

    // While there are still nodes to visit, visit them!
    while( !open.empty() )
    {
        get_first( open, N );
        mark.data[N.h.m][N.h.n].f = -1;
        visited.push_back( N );
        stats.nodes_removed++;
        
        // If we're at the goal, then exit
        if( N.h == B )
            break;

        // If we've looked too long, then exit
        if( stats.nodes_removed >= heuristic.abort_path )
        {
            // Select a good element of OPEN
            for( Container::iterator i = open.begin(); i != open.end(); ++i )
            {
                if( (*i).hval*2 + (*i).gval < N.hval*2 + N.gval )
                    N = *i;
            }
            
            B = N.h;
            break;
        }

        // Every other column gets a different order of searching dirs
        // (Alternatively, you could pick one at random).  I don't want
        // to be too biased by my choice of order in which I look at the
        // neighboring grid spots.
        int directions1[6] = {0,1,2,3,4,5};
        int directions2[6] = {5,4,3,2,1,0};
        int *directions;
        if( (N.h.m+N.h.n) % 2 == 0 )
            directions = directions1;
        else
            directions = directions2;

        // Look at your neighbors.
        for( int dci = 0; dci < 6; ++dci )
        {
            HexDirection d = HexDirection(directions[dci]);
            HexCoord hn = Neighbor( N.h, d );
            // If it's off the end of the map, then don't keep scanning
            if( !map.valid(hn) )
                continue;

            int k = heuristic.kost(map, N.h, d, hn);
            Node N2;
            N2.h = hn;
            N2.gval = N.gval + k;
            N2.hval = heuristic.dist( map, hn, B );
            // If this spot (hn) hasn't been visited, its mark is DirNone
            if( mark.data[hn.m][hn.n].direction == DirNone )
            {
                // The space is not marked
                mark.data[hn.m][hn.n].direction = ReverseDirection(d);
                mark.data[hn.m][hn.n].f = N2.gval+N2.hval;
                mark.data[hn.m][hn.n].g = N2.gval;
                open.push_back( N2 );
                push_heap( open.begin(), open.end(), comp );
                stats.nodes_added++;
            }
            else
            {                
                // We know it's in OPEN or CLOSED...
                if( in_open(hn) )
                {
                    // It's in OPEN, so figure out whether g is better
                    if( N2.gval < mark.data[hn.m][hn.n].g )
                    {
                        // Search for hn in open
                        Container::iterator find1 = find_in_open( hn );
                        assert( find1 != open.end() );
                        
                        // Replace *find1's gval with N2.gval in the list&map
                        mark.data[hn.m][hn.n].direction = ReverseDirection(d);
                        mark.data[hn.m][hn.n].g = N2.gval;
                        mark.data[hn.m][hn.n].f = N2.gval+N2.hval;
                        (*find1).gval = N2.gval;
                        // This is allowed but it's not obvious why:
                        push_heap( open.begin(), find1+1, comp );
                        // (ask Amit if you're curious about it)

                        // This next step is not needed for most games
                        propagate_down( *find1 );
                    }
                }
            }
        }
    }

    if( N.h == B && N.gval < MAXIMUM_PATH_LENGTH )
    {
        stats.path_cost = N.gval;
        // We have found a path, so let's copy it into `path'
        HexCoord h = B;
        while( h != A )
        {
            HexDirection dir = mark.data[h.m][h.n].direction;
            path.push_back( h );
            h = Neighbor( h, dir );
            stats.path_length++;
        }
        path.push_back( A );
        // path now contains the hexes in which the unit must travel ..
        // backwards (like a stack)
    }
    else
    {
        // No path
    }

    stats.nodes_left = open.size();
    stats.nodes_visited = visited.size();
}

////////////////////////////////////////////////////////////////////////
// Specific instantiations of A* for different purposes

// UnitMovement is for moving units (soldiers, builders, firefighters)
struct UnitMovement
{
    HexCoord source;
    Unit* unit;
    int abort_path;
    
    inline static int dist( const HexCoord& a, const HexCoord& b )
    {
        // The **Manhattan** distance is what should be used in A*'s heuristic
        // distance estimate, *not* the straight-line distance.  This is because
        // A* wants to know the estimated distance for its paths, which involve
        // steps along the grid.  (Of course, if you assign 1.4 to the cost of
        // a diagonal, then you should use a distance function close to the
        // real distance.)

        // Here I compute a ``Manhattan'' distance for hexagons.  Nifty, eh?
        int a1 = 2*a.m;
        int a2 =  2*a.n+a.m%2 - a.m;
        int a3 = -2*a.n-a.m%2 - a.m; // == -a1-a2
        int b1 = 2*b.m;
        int b2 =  2*b.n+b.m%2 - b.m;
        int b3 = -2*b.n-b.m%2 - b.m; // == -b1-b2

        // One step on the map is 10 in this function
        return 5*max(abs(a1-b1), max(abs(a2-b2), abs(a3-b3)));
    }

    inline int dist( Map& m, const HexCoord& a, const HexCoord& b )
    {
        double dx1 = a.x() - b.x();
        double dy1 = a.y() - b.y();
        double dx2 = source.x() - b.x();
        double dy2 = source.y() - b.y();
        double cross = dx1*dy2-dx2*dy1;
        if( cross < 0 ) cross = -cross;

        return dist( a, b ) + int(cross/20000);
    }
    
    inline int kost( Map& m, const HexCoord& a, 
                     HexDirection d, const HexCoord& b, int pd = -1 )
    {
        // This is the cost of moving one step.  To get completely accurate
        // paths, this must be greater than or equal to the change in the
        // distance function when you take a step.

        if( pd == -1 ) pd = path_div;
        
        // Check for neighboring moving obstacles
        int occ = m.occupied_[b];
        if( ( occ != -1 && m.units[occ] != unit ) &&
            ( !m.units[occ]->moving() || ( source == a && d != DirNone ) ) )
                return MAXIMUM_PATH_LENGTH;

        // Roads are faster (twice as fast), and cancel altitude effects
        Terrain t1 = m.terrain(a);
        Terrain t2 = m.terrain(b);
        //        int rd = int((t2==Road||t2==Bridge)&&(t1==Road||t2==Bridge));
        // It'd be better theoretically for roads to work only when both
        // hexes are roads, BUT the path finder works faster when
        // it works just when the destination is a road, because it can
        // just step onto a road and know it's going somewhere, as opposed
        // to having to step on the road AND take another step.
        int rd = int(t2==Road || t2==Bridge);
        int rdv = ( 5 - 10 * rd ) * (pd - 3) / 5;
        // Slow everyone down on gates, canals, or walls
        if( t2 == Gate || t2 == Canal )
            rdv += 50;
        if( t2 == Wall )
            rdv += 150;
        // Slow down everyone on water, unless it's on a bridge
        if( t2 != Bridge && m.water(b) > 0 )
            rdv += 30;
        // If there's no road, I take additional items into account
        if( !rd )
        {
            // One thing we can do is penalize for getting OFF a road
            if( t1==Road || t1==Bridge )
                rdv += 15;
            // I take the difference in altitude and use that as a cost,
            // rounded down, which means that small differences cost 0
            int da = (m.altitude(b)-m.altitude(a))/ALTITUDE_SCALE;
            if( da > 0 )
                rdv += da * (pd-3);
        }
        return 10 + rdv;
    }
};

// Some useful functions are exported to be used without the pathfinder

int hex_distance( HexCoord a, HexCoord b )
{
    return UnitMovement::dist( a, b );
}

int movement_cost( Map& m, HexCoord a, HexCoord b, Unit* unit )
{
    UnitMovement um;
    um.unit = unit;
    return um.kost( m, a, DirNone, b, 8 );
}

// BuildingMovement is for drawing straight lines (!)
struct BuildingMovement
{
    HexCoord source;
    int abort_path;
    
    inline int dist( Map& m, const HexCoord& a, const HexCoord& b )
    {
        double dx1 = a.x() - b.x();
        double dy1 = a.y() - b.y();
        double dd1 = dx1*dx1+dy1*dy1;
        // The cross product will be high if two vectors are not colinear
        // so we can calculate the cross product of [current->goal] and
        // [source->goal] to see if we're staying along the [source->goal]
        // vector.  This will help keep us in a straight line.
        double dx2 = source.x() - b.x();
        double dy2 = source.y() - b.y();
        double cross = dx1*dy2-dx2*dy1;
        if( cross < 0 ) cross = -cross;
        return int( dd1 + cross );
    }

    inline int kost( Map& m, const HexCoord& a, 
                     HexDirection d, const HexCoord& b )
    {
        return 0;
    }
};

// Flat Canal movement tries to find a path for a canal that is not too steep
struct FlatCanalPath: public UnitMovement
{
    int kost( Map& m, const HexCoord& a, 
                     HexDirection d, const HexCoord& b )
    {
        // Try to minimize the slope
        int a0 = m.altitude(a);
        int bda = 0;
        for( int dir = 0; dir < 6; ++dir )
        {            
            int da = a0-m.altitude( Neighbor(a,HexDirection(dir)) );
            if( da > bda ) bda = da;
        }
        return 1 + 100*bda*bda;
    }
};

//////////////////////////////////////////////////////////////////////
// These functions call AStar with the proper heuristic object

PathStats FindUnitPath( Map& map, HexCoord A, HexCoord B, 
                        vector<HexCoord>& path, Unit* unit, int cutoff )
{
    UnitMovement um;    
    um.source = A;
    um.unit = unit;
    um.abort_path = cutoff * hex_distance(A,B) / 10;

    AStar<UnitMovement> finder( um, map, A, B );

    // If the goal node is unreachable, don't even try
    if( um.kost( map, A, DirNone, B ) == MAXIMUM_PATH_LENGTH )
    {
        // Check to see if a neighbor is reachable.  This is specific
        // to SimBlob and not something for A* -- I want to find a path
        // to a neighbor if the original goal was unreachable (perhaps it
        // is occupied or unpassable).
        int cost = MAXIMUM_PATH_LENGTH;
        HexCoord Bnew = B;
        for( int d = 0; d < 6; ++d )
        {
            HexCoord hn = Neighbor( B, HexDirection(d) );
            int c = um.kost( map, A, DirNone, hn );
            if( c < cost )
            {
                // This one is closer, hopefully
                Bnew = B;
                cost = c;
            }
        }
        // New goal
        B = Bnew;

        if( cost == MAXIMUM_PATH_LENGTH )
        {
            // No neighbor was good
            return finder.stats;
        }
    }

    finder.find_path( path );
    return finder.stats;
}

PathStats FindBuildPath( Map& map, HexCoord A, HexCoord B,
                         vector<HexCoord>& path )
{
    BuildingMovement bm;
    bm.source = A;

    AStar<BuildingMovement> finder( bm, map, A, B );
    finder.find_path( path );
    return finder.stats;
}

PathStats FindCanalPath( Map& map, HexCoord A, HexCoord B,
                         vector<HexCoord>& path )
{
    FlatCanalPath fcp;
    fcp.source = A;

    AStar<FlatCanalPath> finder( fcp, map, A, B );
    finder.find_path( path );
    return finder.stats;
}