#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <cassert>
#include <cstdint>
#include <vector>
#include <map>
#include <vector>
enum{InvalidPosition = 0xFFFFFFFF};
class Entry
{
public:
enum Type_t
{
TypeWall, ///墙
TypeRoad,///路
TypeNPC,///NPC
TypeEnemy,/// 怪物
};
enum Position_t
{
PositionNormal = 1,
PositionOpenList,
PositionCloseList,
};
private:
struct Extra
{
public:
Extra():
m_position_(PositionNormal),
m_parent_(InvalidPosition),
m_gval_(InvalidPosition),
m_hval_(InvalidPosition),
m_path_flag_(false)
{}
public:
Position_t m_position_;
uint32_t m_parent_;
uint32_t m_gval_;
uint32_t m_hval_;
bool m_path_flag_;
};
public:
Entry(Type_t type,uint32_t index):
m_type_(type),
m_index_(index),
m_extra_(NULL)
{
}
~Entry()
{
if(m_extra_)
delete m_extra_;
}
inline Type_t Type() const
{
return this->m_type_;
}
inline uint32_t Index() const
{
return this->m_index_;
}
inline Position_t Position() const
{
return m_extra_?m_extra_->m_position_:PositionNormal;
}
inline void SetPosition(Position_t position)
{
assert(m_extra_);
m_extra_->m_position_ = position;
}
inline uint32_t FVal() const
{
assert(m_extra_);
return m_extra_->m_gval_ + m_extra_->m_hval_;
}
inline void InitFinder(const Entry& end,uint32_t width)
{
assert(!m_extra_);
uint32_t x1_ = this->m_index_%width;
uint32_t y1_ = this->m_index_/width;
uint32_t x2_ = end.m_index_%width;
uint32_t y2_ = end.m_index_/width;
m_extra_ = new Extra;
// 这里不能用std::abs (unsigned)
m_extra_->m_hval_ = ((x1_ >= x2_)?(x1_ - x2_):(x2_ - x1_)) +
((y1_ >= y2_)?(y1_ - y2_):(y2_ - y1_));
}
inline void SetParent(const Entry& parent,uint32_t width)
{
assert(m_extra_ && parent.m_extra_);
uint32_t x1_ = this->m_index_%width;
uint32_t y1_ = this->m_index_/width;
uint32_t x2_ = parent.m_index_%width;
uint32_t y2_ = parent.m_index_/width;
// 这里不能用std::abs (unsigned)
uint32_t gval_ = ((x1_ >= x2_)?(x1_ - x2_):(x2_ - x1_)) +
((y1_ >= y2_)?(y1_ - y2_):(y2_ - y1_));
// 第一次设置parent
if(m_extra_->m_parent_ == InvalidPosition)
{
m_extra_->m_parent_ = parent.Index();
m_extra_->m_gval_ = gval_;
if(parent.Index() != this->Index())
{
m_extra_->m_gval_ += parent.m_extra_->m_gval_;
}
}
else
{
// 是否需要重新调整
if(m_extra_->m_gval_ > parent.m_extra_->m_gval_ + gval_)
{
m_extra_->m_parent_ = parent.Index();
m_extra_->m_gval_ = parent.m_extra_->m_gval_ + gval_;
}
}
}
uint32_t Parent() const
{
return m_extra_?m_extra_->m_parent_:InvalidPosition;
}
inline void FiniFinder()
{
assert(m_extra_);
delete m_extra_;
m_extra_ = NULL;
}
inline void SetPathFlag()
{
assert(m_extra_);
m_extra_->m_path_flag_ = true;
}
inline bool IsPath() const
{
return m_extra_?m_extra_->m_path_flag_:false;
}
private:
const Type_t m_type_;
const uint32_t m_index_;
Extra* m_extra_;
};
class Map
{
enum Direction_t
{
Direction_Left,
Direction_Right,
Direction_Top,
Direction_Bottom,
};
typedef std::vector<Entry> MapData;
typedef MapData::iterator MapDataIter;
typedef std::multimap<uint32_t,uint32_t> OpenList;
typedef std::vector<uint32_t> CloseList;
public:
Map():
m_init_(false),
m_width_(0),
m_height_(0),
m_start_(InvalidPosition),
m_end_(InvalidPosition)
{
}
/// 从字符文本中初始化
bool InitFromFile(const char* path)
{
assert(path && !m_init_);
FILE* fp_ = fopen(path,"r");
if(!fp_)
{
printf("open file %s fail\n",path);
return false;
}
// 也就是最大只能有100*100(或者等效）的地图
uint8_t buf_[10240];
size_t readed_ = fread(buf_,1,sizeof(buf_),fp_);
if(ferror(fp_))
{
printf("fread file %s fail\n",path);
fclose(fp_);
return false;
}
// 分析内容
assert(readed_ <= sizeof(buf_));
uint8_t* tmp_buf_ = new uint8_t[readed_];
size_t width_ = 0,height_ = 0;
size_t cur_width_ = 0;
size_t valid_cnt_ = 0;
for(size_t i = 0;i < readed_;i++)
{
if(buf_[i] == '.' || buf_[i] == '+' ||
buf_[i] == 's' || buf_[i] == 'd')
{
// 判断开始结束
if(buf_[i] == 's')
{
// 这里直接做断言（判断重复开始/结束位置），不做判断（下同）
assert(InvalidPosition == this->m_start_);
this->m_start_ = valid_cnt_;
buf_[i] = '.';
}
else if(buf_[i] == 'd')
{
assert(InvalidPosition == this->m_end_);
this->m_end_ = valid_cnt_;
buf_[i] = '.';
}
cur_width_ ++;
*(tmp_buf_ + valid_cnt_) = buf_[i];
valid_cnt_ ++;
}
else if(buf_[i] == '\n')
{
// 第一行决定宽度
if(!width_)
{
assert(cur_width_);
width_ = cur_width_;
}
// 后续宽度必须一致
else if(width_ != cur_width_)
{
delete [] tmp_buf_;
fclose(fp_);
return false;
}
cur_width_ = 0;
height_ ++;
}
}
// 是否有开始，结束点（由于符号的设计，开始结束点不可能在墙上，也不可能重复）
if(this->m_start_ == InvalidPosition ||
this->m_end_ == InvalidPosition)
{
printf("there is NO start(%u)/end(%u) position\n",this->m_start_,this->m_end_);
delete [] tmp_buf_;
fclose(fp_);
return false;
}
// 尺寸
m_width_ = width_;
m_height_ = height_;
// 数据
//m_map_data_.resize(valid_cnt_);
//memcpy(&(m_map_data_[0]),tmp_buf_,valid_cnt_);
Entry::Type_t type_;
for(size_t i = 0;i < valid_cnt_;i++)
{
if(tmp_buf_[i] == '.')
type_ = Entry::TypeRoad;
else
type_ = Entry::TypeWall;
m_map_data_.push_back(Entry(type_,i));
}
delete [] tmp_buf_;
fclose(fp_);
m_init_ = true;
return true;
}
/// 打印
void Print()
{
assert(this->m_init_);
size_t cnt_ = 0;
for(MapDataIter iter_ = m_map_data_.begin();
iter_ != m_map_data_.end();
++ iter_)
{
if(cnt_ == this->m_start_)
{
printf("s");
}
else if(cnt_ == this->m_end_)
{
printf("d");
}
else if(iter_->IsPath())
{
printf("-");
}
else if(iter_->Type() == Entry::TypeRoad)
{
printf(".");
}
else
{
printf("+");
}
cnt_ ++;
if(cnt_%m_width_ == 0)
printf("\n");
}
}
/// 寻路
inline bool FindWay()
{
return FindWayImp(m_start_,m_end_);
}
void EndFindWay()
{
for(OpenList::iterator iter_ = m_openlist_.begin();
iter_ != m_openlist_.end();
++ iter_)
{
m_map_data_[iter_->second].FiniFinder();
}
// 清空信息
m_openlist_.clear();
for(CloseList::iterator iter_ = m_closelist_.begin();
iter_ != m_closelist_.end();
++ iter_)
{
m_map_data_[*iter_].FiniFinder();
}
m_closelist_.clear();
}
private:
void Add2Openlist(Entry& entry)
{
entry.SetPosition(Entry::PositionOpenList);
m_openlist_.insert(OpenList::value_type(entry.FVal(),entry.Index()));
}
void Add2Closelist(Entry& entry)
{
entry.SetPosition(Entry::PositionCloseList);
m_closelist_.push_back(entry.Index());
}
uint32_t GetNeighbor(Direction_t direction,uint32_t cur)
{
if(direction == Direction_Left)
{
if(cur%m_width_ > 0)
return cur - 1;
}
else if(direction == Direction_Right)
{
if(cur%m_width_ + 1 < m_width_)
return cur + 1;
}
else if(direction == Direction_Top)
{
if(cur/m_width_ > 0)
return cur - m_width_;
}
else if(direction == Direction_Bottom)
{
if(cur/m_width_ + 1 < m_height_)
return cur + m_width_;
}
return InvalidPosition;
}
bool ProcessNeighborEx(Direction_t direction,uint32_t cur,uint32_t end)
{
uint32_t index_ = GetNeighbor(direction,cur);
if(index_ == InvalidPosition)
return false;
// 排除障碍物
Entry& entry_ = m_map_data_[index_];
if(entry_.Type() != Entry::TypeRoad)
return false;
// 排除在closelist中
if(entry_.Position() == Entry::PositionCloseList)
return false;
// 如果在openlist中
if(entry_.Position() == Entry::PositionOpenList)
{
// 重新计算G值
entry_.SetParent(m_map_data_[cur],m_width_);
printf("update neighbor %u (%uX%u) %u\n",
index_,index_%m_width_,index_/m_width_,entry_.FVal());
}
else
{
// 全新的
entry_.InitFinder(m_map_data_[end],m_width_);
entry_.SetParent(m_map_data_[cur],m_width_);
this->Add2Openlist(entry_);
printf("add neighbor %u (%uX%u) %u\n",
index_,index_%m_width_,index_/m_width_,entry_.FVal());
}
return index_ == end;
}
bool FindWayImp(uint32_t start,uint32_t end)
{
assert(this->m_init_);
assert(start != end);
assert(start < m_map_data_.size() &&
end <= m_map_data_.size());
// 先放入Open列表
{
Entry& start_ = m_map_data_[start];
start_.InitFinder(m_map_data_[end],m_width_);
start_.SetParent(start_,m_width_);
this->Add2Openlist(start_);
}
uint32_t cur_index_;
do{
if(m_openlist_.empty())
{
printf("can NOT find path from %u to %u\n",start,end);
return false;
}
/// 取最小的f值，并转移到closelist
cur_index_ = InvalidPosition;
{
OpenList::iterator iter_ = m_openlist_.begin();
cur_index_ = iter_->second;
printf("process %u (%uX%u) %u\n",
cur_index_,cur_index_%m_width_,cur_index_/m_width_,iter_->first);
m_openlist_.erase(iter_);
this->Add2Closelist(m_map_data_[cur_index_]);
}
if(ProcessNeighborEx(Direction_Left,cur_index_,end) ||
ProcessNeighborEx(Direction_Right,cur_index_,end) ||
ProcessNeighborEx(Direction_Top,cur_index_,end) ||
ProcessNeighborEx(Direction_Bottom,cur_index_,end))
break;
}while(true);
// 回溯路径
while(cur_index_ != start)
{
m_map_data_[cur_index_].SetPathFlag();
cur_index_ = m_map_data_[cur_index_].Parent();
};
return true;
}
private:
bool m_init_;
uint32_t m_width_;
uint32_t m_height_;
MapData m_map_data_;
uint32_t m_start_;
uint32_t m_end_;
OpenList m_openlist_;
CloseList m_closelist_;
};
int main()
{
Map map_;
if(!map_.InitFromFile("map.txt"))
return -1;
map_.Print();
map_.FindWay();
map_.Print();
map_.EndFindWay();
map_.Print();
map_.FindWay();
map_.Print();
return 0;
}