├── Classes ├── Singleton.cpp ├── NonCopyable.h ├── BlockAllocator.h ├── Singleton.h ├── AStar.h ├── BlockAllocator.cpp └── AStar.cpp ├── README.md ├── AStar.sln ├── Main.cpp ├── AStar.vcxproj.filters └── AStar.vcxproj /Classes/Singleton.cpp: -------------------------------------------------------------------------------- 1 | #include "Singleton.h" 2 | 3 | SingletonBase::InstanceTable SingletonBase::s_instance_table_; -------------------------------------------------------------------------------- /Classes/NonCopyable.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | class NonCopyable 4 | { 5 | protected: 6 | NonCopyable() = default; 7 | ~NonCopyable() = default; 8 | 9 | private: 10 | NonCopyable(const NonCopyable&) = delete; 11 | NonCopyable& operator=(const NonCopyable&) = delete; 12 | }; -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # AStar 2 | 3 | # 测试环境 4 | vs2013 5 | photo pc-info_zps1p3tevv0.png 6 | 7 | 测试数据 8 | =================================== 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 |
地图大小允许斜走耗时
100*100否0.001s
100*100是0.000s
1000*1000否0.091s
1000*1000是0.004s
42 | -------------------------------------------------------------------------------- /AStar.sln: -------------------------------------------------------------------------------- 1 | 2 | Microsoft Visual Studio Solution File, Format Version 12.00 3 | # Visual Studio 2013 4 | VisualStudioVersion = 12.0.31101.0 5 | MinimumVisualStudioVersion = 10.0.40219.1 6 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "AStar", "AStar.vcxproj", "{21E1529F-49A8-4FEE-B157-62603C623CF0}" 7 | EndProject 8 | Global 9 | GlobalSection(SolutionConfigurationPlatforms) = preSolution 10 | Debug|Win32 = Debug|Win32 11 | Release|Win32 = Release|Win32 12 | EndGlobalSection 13 | GlobalSection(ProjectConfigurationPlatforms) = postSolution 14 | {21E1529F-49A8-4FEE-B157-62603C623CF0}.Debug|Win32.ActiveCfg = Debug|Win32 15 | {21E1529F-49A8-4FEE-B157-62603C623CF0}.Debug|Win32.Build.0 = Debug|Win32 16 | {21E1529F-49A8-4FEE-B157-62603C623CF0}.Release|Win32.ActiveCfg = Release|Win32 17 | {21E1529F-49A8-4FEE-B157-62603C623CF0}.Release|Win32.Build.0 = Release|Win32 18 | EndGlobalSection 19 | GlobalSection(SolutionProperties) = preSolution 20 | HideSolutionNode = FALSE 21 | EndGlobalSection 22 | GlobalSection(Performance) = preSolution 23 | HasPerformanceSessions = true 24 | EndGlobalSection 25 | EndGlobal 26 | -------------------------------------------------------------------------------- /Classes/BlockAllocator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Singleton.h" 4 | #include "NonCopyable.h" 5 | 6 | const int g_chunk_size = 16 * 1024; 7 | const int g_max_block_size = 640; 8 | const int g_block_sizes = 14; 9 | const int g_chunk_array_increment = 128; 10 | 11 | /// This is a small object allocator used for allocating small 12 | /// objects that persist for more than one time step. 13 | /// See: http://www.codeproject.com/useritems/Small_Block_Allocator.asp 14 | class BlockAllocator : public NonCopyable 15 | { 16 | public: 17 | BlockAllocator(); 18 | ~BlockAllocator(); 19 | 20 | public: 21 | void* Allocate(int size); 22 | void Free(void *p, int size); 23 | void Clear(); 24 | 25 | private: 26 | int num_chunk_count_; 27 | int num_chunk_space_; 28 | struct Chunk* chunks_; 29 | struct Block* free_lists_[g_block_sizes]; 30 | static int block_sizes_[g_block_sizes]; 31 | static char s_block_size_lookup_[g_max_block_size + 1]; 32 | static bool s_block_size_lookup_initialized_; 33 | }; 34 | 35 | class SOA final : public Singleton < SOA >, public BlockAllocator 36 | { 37 | SINGLETON_DEFAULT(SOA); 38 | }; -------------------------------------------------------------------------------- /Main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "AStar.h" 3 | #include 4 | using namespace std; 5 | 6 | int main() 7 | { 8 | char astar_map[1000][1000] = 9 | { 10 | { 0, 1, 0, 0, 0, 1, 0, 0, 0, 0 }, 11 | { 0, 0, 0, 1, 0, 1, 0, 1, 0, 1 }, 12 | { 1, 1, 1, 1, 0, 1, 0, 1, 0, 1 }, 13 | { 0, 0, 0, 1, 0, 0, 0, 1, 0, 1 }, 14 | { 0, 1, 0, 1, 1, 1, 1, 1, 0, 1 }, 15 | { 0, 1, 0, 0, 0, 0, 0, 0, 0, 1 }, 16 | { 0, 1, 1, 1, 1, 1, 1, 1, 1, 1 }, 17 | { 0, 0, 0, 0, 1, 0, 0, 0, 1, 0 }, 18 | { 1, 1, 0, 0, 1, 0, 1, 0, 0, 0 }, 19 | { 0, 0, 0, 0, 0, 0, 1, 0, 1, 0 }, 20 | }; 21 | 22 | auto CanReach = [&](const Point &grid)->bool 23 | { 24 | return astar_map[grid.row][grid.col] == 0; 25 | }; 26 | 27 | AStarDef def; 28 | def.row = 1000; 29 | def.col = 1000; 30 | def.can_reach = CanReach; 31 | def.start_point = Point(0, 0); 32 | def.end_point = Point(999, 999); 33 | def.allow_corner = false; 34 | 35 | clock_t start, end; 36 | start = clock(); 37 | 38 | AStar astar; 39 | auto path = astar.Search(def); 40 | 41 | end = clock(); 42 | cout << "Run time: " << (double)(end - start) / CLOCKS_PER_SEC << "s" << endl; 43 | 44 | system("pause"); 45 | return 0; 46 | } 47 | -------------------------------------------------------------------------------- /AStar.vcxproj.filters: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} 6 | rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms 7 | 8 | 9 | {cb55168c-26d7-43e3-a807-7c5d7ad29c6a} 10 | 11 | 12 | 13 | 14 | Classes 15 | 16 | 17 | Classes 18 | 19 | 20 | Classes 21 | 22 | 23 | 24 | 25 | 26 | Classes 27 | 28 | 29 | Classes 30 | 31 | 32 | Classes 33 | 34 | 35 | Classes 36 | 37 | 38 | -------------------------------------------------------------------------------- /Classes/Singleton.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | class SingletonBase 6 | { 7 | class InstanceTable : public std::list < SingletonBase * > 8 | { 9 | public: 10 | InstanceTable() 11 | : cleared_(false) 12 | { 13 | 14 | }; 15 | 16 | ~InstanceTable() 17 | { 18 | cleared_ = true; 19 | while (!empty()) 20 | { 21 | delete back(); 22 | pop_back(); 23 | } 24 | } 25 | 26 | public: 27 | bool cleared_; 28 | }; 29 | 30 | protected: 31 | SingletonBase() 32 | { 33 | s_instance_table_.push_back(this); 34 | } 35 | 36 | virtual ~SingletonBase() 37 | { 38 | if (!s_instance_table_.cleared_) 39 | { 40 | s_instance_table_.remove(this); 41 | } 42 | } 43 | 44 | private: 45 | static InstanceTable s_instance_table_; 46 | }; 47 | 48 | template 49 | class Singleton : public SingletonBase 50 | { 51 | public: 52 | static T* GetInstance() 53 | { 54 | if (s_singleton_ == nullptr) 55 | { 56 | s_singleton_ = new (std::nothrow) T(); 57 | } 58 | return s_singleton_; 59 | } 60 | 61 | static void DestroyInstance() 62 | { 63 | if (s_singleton_) 64 | { 65 | delete s_singleton_; 66 | } 67 | } 68 | 69 | protected: 70 | Singleton() 71 | { 72 | 73 | }; 74 | 75 | virtual ~Singleton() 76 | { 77 | s_singleton_ = nullptr; 78 | }; 79 | 80 | private: 81 | static T* s_singleton_; 82 | }; 83 | 84 | template T* Singleton::s_singleton_ = nullptr; 85 | 86 | #define SINGLETON(_class_) \ 87 | private: \ 88 | _class_(); \ 89 | ~_class_(); \ 90 | friend class Singleton<_class_>; 91 | 92 | #define SINGLETON_DEFAULT(_class_) \ 93 | private: \ 94 | _class_() {}; \ 95 | ~_class_() {}; \ 96 | friend class Singleton<_class_>; -------------------------------------------------------------------------------- /AStar.vcxproj: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Debug 6 | Win32 7 | 8 | 9 | Release 10 | Win32 11 | 12 | 13 | 14 | {21E1529F-49A8-4FEE-B157-62603C623CF0} 15 | Win32Proj 16 | AStar 17 | 18 | 19 | 20 | Application 21 | true 22 | v120 23 | Unicode 24 | 25 | 26 | Application 27 | false 28 | v120 29 | true 30 | Unicode 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | true 44 | 45 | 46 | false 47 | 48 | 49 | 50 | 51 | 52 | Level3 53 | Disabled 54 | WIN32;_DEBUG;_CONSOLE;_LIB;%(PreprocessorDefinitions) 55 | .\Classes;%(AdditionalIncludeDirectories) 56 | 57 | 58 | Console 59 | true 60 | 61 | 62 | 63 | 64 | Level3 65 | 66 | 67 | MaxSpeed 68 | true 69 | true 70 | WIN32;NDEBUG;_CONSOLE;_LIB;%(PreprocessorDefinitions) 71 | .\Classes;%(AdditionalIncludeDirectories) 72 | 73 | 74 | Console 75 | true 76 | true 77 | true 78 | true 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | -------------------------------------------------------------------------------- /Classes/AStar.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | Copyright (c) 2015 Zhangpanyi 3 | 4 | Created by Zhangpanyi on 2015 5 | 6 | zhangpanyi@live.com 7 | ****************************************************************************/ 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include "NonCopyable.h" 14 | #include "BlockAllocator.h" 15 | 16 | #define NOTEXIST 0 17 | #define IN_OPENLIST 1 18 | #define IN_CLOSELIST 2 19 | 20 | /** 21 | * 位置 22 | */ 23 | struct Point 24 | { 25 | unsigned short row; 26 | unsigned short col; 27 | 28 | Point() : row(0), col(0) {} 29 | Point(unsigned short row, unsigned short col) : row(row), col(col) {} 30 | 31 | bool operator== (const Point &that) const 32 | { 33 | return row == that.row && col == that.col; 34 | } 35 | 36 | const Point& operator() (unsigned short row, unsigned short col) 37 | { 38 | this->row = row; 39 | this->col = col; 40 | return *this; 41 | } 42 | }; 43 | 44 | /** 45 | * 查询函数 46 | */ 47 | typedef std::function QueryCallBack; 48 | 49 | /** 50 | * A*算法参数定义 51 | */ 52 | struct AStarDef 53 | { 54 | bool allow_corner; 55 | unsigned short row; 56 | unsigned short col; 57 | Point start_point; 58 | Point end_point; 59 | QueryCallBack can_reach; 60 | 61 | AStarDef() : row(0), col(0), can_reach(nullptr), allow_corner(false) {} 62 | }; 63 | 64 | class AStar : public NonCopyable 65 | { 66 | public: 67 | /** 68 | * 格子结点 69 | * 记录g值、h值、和父节点信息 70 | * 使用小对象分配器分配内存 71 | */ 72 | struct Node 73 | { 74 | unsigned short g; 75 | unsigned short h; 76 | Point pos; 77 | int state; 78 | Node* parent; 79 | 80 | int f() const 81 | { 82 | return g + h; 83 | } 84 | 85 | Node(const Point &pos) : g(0), h(0), pos(pos), parent(nullptr), state(NOTEXIST) {} 86 | 87 | void* operator new(std::size_t size) 88 | { 89 | void *ptr = SOA::GetInstance()->Allocate(size); 90 | return ptr; 91 | } 92 | 93 | void operator delete(void* p) throw() 94 | { 95 | if (p) SOA::GetInstance()->Free(p, sizeof(Node)); 96 | } 97 | }; 98 | 99 | public: 100 | AStar(); 101 | ~AStar(); 102 | 103 | public: 104 | /** 105 | * 执行A*搜索 106 | * @ 参数 def A*算法参数定义 107 | * @ 返回搜索路径 108 | */ 109 | std::deque Search(const AStarDef &def); 110 | 111 | private: 112 | /** 113 | * 清理 114 | */ 115 | void Clear(); 116 | 117 | /** 118 | * 初始化 119 | * @ 参数 def A*算法参数定义 120 | */ 121 | void Init(const AStarDef &def); 122 | 123 | /** 124 | * 检测 A*算法参数定义是否有效 125 | * @ 参数 def A*算法参数定义 126 | */ 127 | bool ValidAStarDef(const AStarDef &def); 128 | 129 | private: 130 | /** 131 | * 格子是否存在于开启列表 132 | * @ 参数 Point 位置 133 | * @ 存在返回格子结点的指针，不存在返回nullptr 134 | */ 135 | Node* IsExistInOpenList(const Point &point); 136 | 137 | /** 138 | * 格子是否存在于关闭列表 139 | * @ 参数 Point 位置 140 | * @ 存在返回true，不存在返回false 141 | */ 142 | bool IsExistInCloseList(const Point &point); 143 | 144 | /** 145 | * 查询格子是否可通行 146 | * @ 参数 target_point 目标点 147 | * @ 成功返回true，失败返回false 148 | */ 149 | bool IsCanReach(const Point &target_point); 150 | 151 | /** 152 | * 查询格子是否可到达 153 | * @ 参数 current_point 当前点, target_point 目标点, allow_corner 是否允许斜走 154 | * @ 成功返回true，失败返回false 155 | */ 156 | bool IsCanReached(const Point ¤t_point, const Point &target_point, bool allow_corner); 157 | 158 | /** 159 | * 搜索可通行的格子 160 | * @ 参数 current_point 当前点, allow_corner 是否允许斜走, surround_point 存放搜索结果的数组 161 | */ 162 | void SearchCanReached(const Point ¤t_point, bool allow_corner, std::vector &surround_point); 163 | 164 | /** 165 | * 计算G值 166 | * @ 参数 parent 父节点, current_point 当前点 167 | */ 168 | unsigned int CalculG(Node *parent, const Point ¤t_point); 169 | 170 | /** 171 | * 计算H值 172 | * @ 参数 current_point 当前点, end_point 终点 173 | */ 174 | unsigned int CalculH(const Point ¤t_point, const Point &end_point); 175 | 176 | /** 177 | * 获取节点在开启列表中的索引值 178 | * @ 失败返回-1 179 | */ 180 | int GetIndex(Node *node); 181 | 182 | /** 183 | * 开启列表上滤(二叉堆上滤) 184 | * @ 参数 hole 上滤位置 185 | */ 186 | void PercolateUp(int hole); 187 | 188 | private: 189 | /** 190 | * 当节点存在于开启列表中的处理函数 191 | */ 192 | void FoundNode(Node *current_point, Node *new_point); 193 | 194 | /** 195 | * 当节点不存在于开启列表中的处理函数 196 | */ 197 | void NotFoundNode(Node *current_point, Node *new_point, const Point &end); 198 | 199 | private: 200 | unsigned short num_row_; // 地图行数 201 | unsigned short num_col_; // 地图列数 202 | unsigned int num_map_size_; // 节点地图大小 203 | QueryCallBack query_func_; // 查询函数 204 | std::vector open_list_; // 开启列表 205 | std::vector maps_index_; // 地图索引 206 | }; -------------------------------------------------------------------------------- /Classes/BlockAllocator.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include "BlockAllocator.h" 7 | 8 | int BlockAllocator::block_sizes_[g_block_sizes] = 9 | { 10 | 16, // 0 11 | 32, // 1 12 | 64, // 2 13 | 96, // 3 14 | 128, // 4 15 | 160, // 5 16 | 192, // 6 17 | 224, // 7 18 | 256, // 8 19 | 320, // 9 20 | 384, // 10 21 | 448, // 11 22 | 512, // 12 23 | 640, // 13 24 | }; 25 | char BlockAllocator::s_block_size_lookup_[g_max_block_size + 1]; 26 | bool BlockAllocator::s_block_size_lookup_initialized_; 27 | 28 | struct Chunk 29 | { 30 | int block_size; 31 | Block *blocks; 32 | }; 33 | 34 | struct Block 35 | { 36 | Block *next; 37 | }; 38 | 39 | BlockAllocator::BlockAllocator() 40 | { 41 | assert(g_block_sizes < UCHAR_MAX); 42 | 43 | num_chunk_space_ = g_chunk_array_increment; 44 | num_chunk_count_ = 0; 45 | chunks_ = (Chunk *)malloc(num_chunk_space_ * sizeof(Chunk)); 46 | 47 | memset(chunks_, 0, num_chunk_space_ * sizeof(Chunk)); 48 | memset(free_lists_, 0, sizeof(free_lists_)); 49 | 50 | if (s_block_size_lookup_initialized_ == false) 51 | { 52 | int j = 0; 53 | for (int i = 1; i <= g_max_block_size; ++i) 54 | { 55 | assert(j < g_block_sizes); 56 | if (i <= block_sizes_[j]) 57 | { 58 | s_block_size_lookup_[i] = (char)j; 59 | } 60 | else 61 | { 62 | ++j; 63 | s_block_size_lookup_[i] = (char)j; 64 | } 65 | } 66 | 67 | s_block_size_lookup_initialized_ = true; 68 | } 69 | } 70 | 71 | BlockAllocator::~BlockAllocator() 72 | { 73 | for (int i = 0; i < num_chunk_count_; ++i) 74 | { 75 | ::free(chunks_[i].blocks); 76 | } 77 | 78 | ::free(chunks_); 79 | } 80 | 81 | void* BlockAllocator::Allocate(int size) 82 | { 83 | if (size == 0) 84 | return nullptr; 85 | 86 | assert(0 < size); 87 | 88 | if (size > g_max_block_size) 89 | { 90 | return malloc(size); 91 | } 92 | 93 | int index = s_block_size_lookup_[size]; 94 | assert(0 <= index && index < g_block_sizes); 95 | 96 | if (free_lists_[index]) 97 | { 98 | Block *block = free_lists_[index]; 99 | free_lists_[index] = block->next; 100 | return block; 101 | } 102 | else 103 | { 104 | if (num_chunk_count_ == num_chunk_space_) 105 | { 106 | Chunk *oldChunks = chunks_; 107 | num_chunk_space_ += g_chunk_array_increment; 108 | chunks_ = (Chunk *)malloc(num_chunk_space_ * sizeof(Chunk)); 109 | memcpy(chunks_, oldChunks, num_chunk_count_ * sizeof(Chunk)); 110 | memset(chunks_ + num_chunk_count_, 0, g_chunk_array_increment * sizeof(Chunk)); 111 | ::free(oldChunks); 112 | } 113 | 114 | Chunk *chunk = chunks_ + num_chunk_count_; 115 | chunk->blocks = (Block *)malloc(g_chunk_size); 116 | #if defined(_DEBUG) 117 | memset(chunk->blocks, 0xcd, g_chunk_size); 118 | #endif 119 | int block_size = block_sizes_[index]; 120 | chunk->block_size = block_size; 121 | int block_count = g_chunk_size / block_size; 122 | assert(block_count * block_size <= g_chunk_size); 123 | for (int i = 0; i < block_count - 1; ++i) 124 | { 125 | Block *block = (Block *)((char *)chunk->blocks + block_size * i); 126 | Block *next = (Block *)((char *)chunk->blocks + block_size * (i + 1)); 127 | block->next = next; 128 | } 129 | Block *last = (Block *)((char *)chunk->blocks + block_size * (block_count - 1)); 130 | last->next = nullptr; 131 | 132 | free_lists_[index] = chunk->blocks->next; 133 | ++num_chunk_count_; 134 | 135 | return chunk->blocks; 136 | } 137 | } 138 | 139 | void BlockAllocator::Free(void *p, int size) 140 | { 141 | if (size == 0) 142 | { 143 | return; 144 | } 145 | 146 | assert(0 < size); 147 | 148 | if (size > g_max_block_size) 149 | { 150 | ::free(p); 151 | return; 152 | } 153 | 154 | int index = s_block_size_lookup_[size]; 155 | assert(0 <= index && index < g_block_sizes); 156 | 157 | #ifdef _DEBUG 158 | int block_size = block_sizes_[index]; 159 | bool found = false; 160 | for (int i = 0; i < num_chunk_count_; ++i) 161 | { 162 | Chunk *chunk = chunks_ + i; 163 | if (chunk->block_size != block_size) 164 | { 165 | assert((char *)p + block_size <= (char *)chunk->blocks || 166 | (char *)chunk->blocks + g_chunk_size <= (char *)p); 167 | } 168 | else 169 | { 170 | if ((char *)chunk->blocks <= (char *)p && (char *)p + block_size <= (char *)chunk->blocks + g_chunk_size) 171 | { 172 | found = true; 173 | } 174 | } 175 | } 176 | 177 | assert(found); 178 | 179 | memset(p, 0xfd, block_size); 180 | #endif 181 | 182 | Block *block = (Block *)p; 183 | block->next = free_lists_[index]; 184 | free_lists_[index] = block; 185 | } 186 | 187 | void BlockAllocator::Clear() 188 | { 189 | for (int i = 0; i < num_chunk_count_; ++i) 190 | { 191 | ::free(chunks_[i].blocks); 192 | } 193 | 194 | num_chunk_count_ = 0; 195 | memset(chunks_, 0, num_chunk_space_ * sizeof(Chunk)); 196 | 197 | memset(free_lists_, 0, sizeof(free_lists_)); 198 | } -------------------------------------------------------------------------------- /Classes/AStar.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "AStar.h" 4 | 5 | const int kStep = 10; 6 | const int kOblique = 14; 7 | 8 | bool CompHeap(const AStar::Node *a, const AStar::Node *b) 9 | { 10 | return a->f() > b->f(); 11 | } 12 | 13 | AStar::AStar() 14 | : num_row_(0) 15 | , num_col_(0) 16 | , num_map_size_(0) 17 | , query_func_(nullptr) 18 | { 19 | } 20 | 21 | AStar::~AStar() 22 | { 23 | 24 | } 25 | 26 | void AStar::Init(const AStarDef &def) 27 | { 28 | num_row_ = def.row; 29 | num_col_ = def.col; 30 | query_func_ = def.can_reach; 31 | num_map_size_ = num_row_ * num_col_; 32 | 33 | if (!maps_index_.empty()) 34 | { 35 | memset(&maps_index_[0], 0, sizeof(std::vector::value_type) * maps_index_.size()); 36 | } 37 | maps_index_.resize(num_map_size_, nullptr); 38 | } 39 | 40 | void AStar::Clear() 41 | { 42 | unsigned int index = 0; 43 | while (index < num_map_size_) 44 | { 45 | if (maps_index_[index]) 46 | { 47 | delete maps_index_[index]; 48 | maps_index_[index] = nullptr; 49 | } 50 | ++index; 51 | } 52 | 53 | num_row_ = 0; 54 | num_col_ = 0; 55 | open_list_.clear(); 56 | num_map_size_ = 0; 57 | query_func_ = nullptr; 58 | } 59 | 60 | inline AStar::Node* AStar::IsExistInOpenList(const Point &point) 61 | { 62 | Node *node_ptr = maps_index_[point.row * num_row_ + point.col]; 63 | return node_ptr ? (node_ptr->state == IN_OPENLIST ? node_ptr : nullptr) : nullptr; 64 | } 65 | 66 | inline bool AStar::IsExistInCloseList(const Point &point) 67 | { 68 | Node *node_ptr = maps_index_[point.row * num_row_ + point.col]; 69 | return node_ptr ? node_ptr->state == IN_CLOSELIST : false; 70 | } 71 | 72 | bool AStar::IsCanReach(const Point &target_point) 73 | { 74 | if (target_point.col >= 0 && target_point.col < num_col_ && target_point.row >= 0 && target_point.row < num_row_) 75 | { 76 | return query_func_(target_point); 77 | } 78 | else 79 | { 80 | return false; 81 | } 82 | } 83 | 84 | bool AStar::IsCanReached(const Point ¤t_point, const Point &target_point, bool allow_corner) 85 | { 86 | if (!IsCanReach(target_point) || IsExistInCloseList(target_point)) 87 | { 88 | return false; 89 | } 90 | 91 | if (abs(current_point.row + current_point.col - target_point.row - target_point.col) == 1) 92 | { 93 | return true; 94 | } 95 | else if (allow_corner) 96 | { 97 | return (IsCanReach(Point(current_point.col + target_point.col - current_point.col, current_point.row)) 98 | && IsCanReach(Point(current_point.col, current_point.row + target_point.row - current_point.row))); 99 | } 100 | 101 | return false; 102 | } 103 | 104 | void AStar::SearchCanReached(const Point ¤t_point, bool allow_corner, std::vector &surround_point) 105 | { 106 | Point target; 107 | surround_point.clear(); 108 | 109 | for (int row = current_point.row - 1; row <= current_point.row + 1; ++row) 110 | { 111 | for (int col = current_point.col - 1; col <= current_point.col + 1; ++col) 112 | { 113 | if (IsCanReached(current_point, target(row, col), allow_corner)) 114 | { 115 | surround_point.push_back(target); 116 | } 117 | } 118 | } 119 | } 120 | 121 | inline unsigned int AStar::CalculG(Node *parent, const Point ¤t_point) 122 | { 123 | unsigned int g_value = ((abs(current_point.row + current_point.col - parent->pos.row - parent->pos.col)) == 2 ? kOblique : kStep); 124 | g_value += parent->g; 125 | return g_value; 126 | } 127 | 128 | inline unsigned int AStar::CalculH(const Point ¤t_point, const Point &end_point) 129 | { 130 | unsigned int h_value = abs(end_point.row + end_point.col - current_point.row - current_point.col); 131 | return h_value * kStep; 132 | } 133 | 134 | int AStar::GetIndex(Node *node) 135 | { 136 | unsigned int index = 0; 137 | const unsigned int size = open_list_.size(); 138 | 139 | while (index < size) 140 | { 141 | if (open_list_[index]->pos == node->pos) 142 | { 143 | return index; 144 | } 145 | ++index; 146 | } 147 | 148 | return -1; 149 | } 150 | 151 | void AStar::PercolateUp(int hole) 152 | { 153 | unsigned int parent = 0; 154 | while (hole > 1) 155 | { 156 | parent = (hole - 1) / 2; 157 | if (open_list_[hole]->f() < open_list_[parent]->f()) 158 | { 159 | std::swap(open_list_[hole], open_list_[parent]); 160 | hole = parent; 161 | } 162 | else 163 | { 164 | return; 165 | } 166 | } 167 | } 168 | 169 | void AStar::FoundNode(Node *current_point, Node *new_point) 170 | { 171 | unsigned int g_value = CalculG(current_point, new_point->pos); 172 | 173 | if (g_value < new_point->g) 174 | { 175 | new_point->g = g_value; 176 | new_point->parent = current_point; 177 | PercolateUp(GetIndex(new_point)); 178 | } 179 | } 180 | 181 | void AStar::NotFoundNode(Node *current_point, Node *new_point, const Point &end) 182 | { 183 | new_point->parent = current_point; 184 | new_point->g = CalculG(current_point, new_point->pos); 185 | new_point->h = CalculH(new_point->pos, end); 186 | 187 | Node *&node_ptr = maps_index_[new_point->pos.row * num_row_ + new_point->pos.col]; 188 | assert(node_ptr == nullptr); 189 | node_ptr = new_point; 190 | node_ptr->state = IN_OPENLIST; 191 | 192 | open_list_.push_back(new_point); 193 | std::push_heap(open_list_.begin(), open_list_.end(), CompHeap); 194 | } 195 | 196 | inline bool AStar::ValidAStarDef(const AStarDef &def) 197 | { 198 | return (def.can_reach 199 | && (def.col >= 0 && def.row >= 0) 200 | && (def.start_point.col >= 0 && def.start_point.col < def.col) 201 | && (def.start_point.row >= 0 && def.start_point.row < def.row) 202 | && (def.end_point.col >= 0 && def.end_point.col < def.col) 203 | && (def.end_point.row >= 0 && def.end_point.row < def.row) 204 | ); 205 | } 206 | 207 | std::deque AStar::Search(const AStarDef &def) 208 | { 209 | std::deque search_path; 210 | if (ValidAStarDef(def)) 211 | { 212 | Init(def); 213 | 214 | std::vector around_point; 215 | around_point.reserve(def.allow_corner ? 8 : 4); 216 | 217 | Node *start_node = new Node(def.start_point); 218 | open_list_.push_back(start_node); 219 | 220 | Node *&node_ptr = maps_index_[start_node->pos.row * num_row_ + start_node->pos.col]; 221 | assert(node_ptr == nullptr); 222 | node_ptr = start_node; 223 | node_ptr->state = IN_OPENLIST; 224 | 225 | while (!open_list_.empty()) 226 | { 227 | Node *current_point = open_list_[0]; 228 | std::pop_heap(open_list_.begin(), open_list_.end(), CompHeap); 229 | open_list_.pop_back(); 230 | maps_index_[current_point->pos.row * num_row_ + current_point->pos.col]->state = IN_CLOSELIST; 231 | 232 | SearchCanReached(current_point->pos, def.allow_corner, around_point); 233 | 234 | unsigned int size = around_point.size(); 235 | for (unsigned int index = 0; index < size; ++index) 236 | { 237 | Node *new_point = IsExistInOpenList(around_point[index]); 238 | if (new_point) 239 | { 240 | FoundNode(current_point, new_point); 241 | } 242 | else 243 | { 244 | new_point = new Node(around_point[index]); 245 | NotFoundNode(current_point, new_point, def.end_point); 246 | 247 | if (around_point[index] == def.end_point) 248 | { 249 | while (new_point->parent) 250 | { 251 | search_path.push_front(new_point->pos); 252 | new_point = new_point->parent; 253 | } 254 | goto end_search; 255 | } 256 | } 257 | } 258 | } 259 | 260 | end_search: 261 | Clear(); 262 | } 263 | else 264 | { 265 | Clear(); 266 | assert("Invalid AStarDef!"); 267 | } 268 | 269 | return search_path; 270 | } --------------------------------------------------------------------------------