Initial commit
This commit is contained in:
532
engines/qdengine/qdcore/util/AIAStar.h
Normal file
532
engines/qdengine/qdcore/util/AIAStar.h
Normal file
@@ -0,0 +1,532 @@
|
||||
/* ScummVM - Graphic Adventure Engine
|
||||
*
|
||||
* ScummVM is the legal property of its developers, whose names
|
||||
* are too numerous to list here. Please refer to the COPYRIGHT
|
||||
* file distributed with this source distribution.
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
*/
|
||||
|
||||
//K-D Lab / Balmer
|
||||
#pragma once
|
||||
|
||||
#include "common/multimap.h"
|
||||
|
||||
|
||||
///////////////////////////AIAStar/////////////////////
|
||||
//AIAStar::FindPath поиск пути из точки from
|
||||
//в точку IsEndPoint.
|
||||
//Боле-менее оптимизированный на случай квадратной сетки
|
||||
|
||||
/*
|
||||
class Heuristic
|
||||
{
|
||||
float getH(int x,int y);//Предполагаемые затраты на продвижение из pos1 к окончанию
|
||||
float getG(int x1,int y1,int x2,int y2);//Затраты на продвижение из pos1 в pos2
|
||||
bool IsEndPoint(int x,int y);//Рекурсия должна окончиться здесь
|
||||
//то есть класс AIAStar позволяет задавать несколько точек окончания поиска пути
|
||||
};
|
||||
*/
|
||||
|
||||
namespace QDEngine {
|
||||
|
||||
template<class Heuristic, class TypeH = float>
|
||||
class AIAStar {
|
||||
public:
|
||||
typedef Common::MultiMap<TypeH, Vect2i> type_point_map;
|
||||
|
||||
struct OnePoint {
|
||||
TypeH g;//Затраты на продвижение до этой точки
|
||||
TypeH h;//Предполагаемые затраты на продвижение до финиша
|
||||
int used;
|
||||
OnePoint *parent;
|
||||
bool is_open;
|
||||
|
||||
inline TypeH f() {
|
||||
return g + h;
|
||||
}
|
||||
};
|
||||
protected:
|
||||
int _dx, _dy;
|
||||
OnePoint *_chart;
|
||||
type_point_map _open_map;
|
||||
|
||||
int _is_used_num;//Если _is_used_num==used, то ячейка используется
|
||||
|
||||
int _num_point_examine;//количество посещённых ячеек
|
||||
int _num_find_erase;//Сколько суммарно искали ячейки для удаления
|
||||
Heuristic *_heuristic;
|
||||
public:
|
||||
AIAStar();
|
||||
~AIAStar();
|
||||
|
||||
void init(int dx, int dy);
|
||||
bool findPath(Vect2i from, Heuristic *h, Std::vector<Vect2i> &path, int directions_count = 8);
|
||||
void getStatistic(int *num_point_examine, int *num_find_erase);
|
||||
|
||||
//Debug
|
||||
OnePoint *getInternalBuffer() {
|
||||
return _chart;
|
||||
};
|
||||
int getUsedNum() {
|
||||
return _is_used_num;
|
||||
}
|
||||
protected:
|
||||
void clear();
|
||||
inline Vect2i posBy(OnePoint *p) {
|
||||
int offset = p - _chart;
|
||||
Vect2i pos;
|
||||
pos.x = offset % _dx;
|
||||
pos.y = offset / _dx;
|
||||
return pos;
|
||||
}
|
||||
};
|
||||
|
||||
template<class Heuristic, class TypeH>
|
||||
AIAStar<Heuristic, TypeH>::AIAStar() {
|
||||
_chart = NULL;
|
||||
_heuristic = NULL;
|
||||
_num_find_erase = 0;
|
||||
_dx = _dy = 0;
|
||||
_is_used_num = 0;
|
||||
_num_point_examine = 0;
|
||||
}
|
||||
|
||||
template<class Heuristic, class TypeH>
|
||||
void AIAStar<Heuristic, TypeH>::init(int dx_, int dy_) {
|
||||
_dx = dx_;
|
||||
_dy = dy_;
|
||||
|
||||
int size = _dx * _dy;
|
||||
_chart = new OnePoint[size];
|
||||
clear();
|
||||
}
|
||||
|
||||
template<class Heuristic, class TypeH>
|
||||
void AIAStar<Heuristic, TypeH>::clear() {
|
||||
int size = _dx * _dy;
|
||||
_is_used_num = 0;
|
||||
for (int i = 0; i < size; i++)
|
||||
_chart[i].used = 0;
|
||||
}
|
||||
|
||||
template<class Heuristic, class TypeH>
|
||||
AIAStar<Heuristic, TypeH>::~AIAStar() {
|
||||
delete[] _chart;
|
||||
}
|
||||
|
||||
template<class Heuristic, class TypeH>
|
||||
bool AIAStar<Heuristic, TypeH>::findPath(Vect2i from, Heuristic *hr, Std::vector<Vect2i> &path, int directions_count) {
|
||||
_num_point_examine = 0;
|
||||
_num_find_erase = 0;
|
||||
|
||||
_is_used_num++;
|
||||
_open_map.clear();
|
||||
path.clear();
|
||||
if (_is_used_num == 0)
|
||||
clear();//Для того, чтобы вызвалась эта строчка, необходимо гиганское время
|
||||
assert(from.x >= 0 && from.x < _dx && from.y >= 0 && from.y < _dy);
|
||||
_heuristic = hr;
|
||||
|
||||
OnePoint *p = _chart + from.y * _dx + from.x;
|
||||
p->g = 0;
|
||||
p->h = _heuristic->getH(from.x, from.y);
|
||||
p->used = _is_used_num;
|
||||
p->is_open = true;
|
||||
p->parent = NULL;
|
||||
|
||||
_open_map.insert(typename type_point_map::value_type(p->f(), from));
|
||||
|
||||
const int sx[8] = { 0, -1, 0, +1, -1, +1, +1, -1,};
|
||||
const int sy[8] = {-1, 0, +1, 0, -1, -1, +1, +1 };
|
||||
|
||||
// const int sx[size_child]={ 0,-1, 0,+1};
|
||||
// const int sy[size_child]={-1, 0,+1, 0};
|
||||
|
||||
const int size_child = directions_count;
|
||||
|
||||
while (!_open_map.empty()) {
|
||||
typename type_point_map::iterator low = _open_map.begin();
|
||||
Vect2i pt = (*low).second;
|
||||
OnePoint *parent = _chart + pt.y * _dx + pt.x;
|
||||
|
||||
parent->is_open = false;
|
||||
_open_map.erase(low);
|
||||
|
||||
if (_heuristic->isEndPoint(pt.x, pt.y)) {
|
||||
//сконструировать путь
|
||||
Vect2i vp;
|
||||
while (parent) {
|
||||
vp = posBy(parent);;
|
||||
path.push_back(vp);
|
||||
|
||||
if (parent->parent) {
|
||||
Vect2i pp;
|
||||
pp = posBy(parent->parent);
|
||||
assert(abs(vp.x - pp.x) <= 1 &&
|
||||
abs(vp.y - pp.y) <= 1);
|
||||
}
|
||||
|
||||
parent = parent->parent;
|
||||
}
|
||||
assert(vp.x == from.x && vp.y == from.y);
|
||||
Common::reverse(path.begin(), path.end());
|
||||
return true;
|
||||
}
|
||||
|
||||
//для каждого наследника child узла parent
|
||||
for (int i = 0; i < size_child; i++) {
|
||||
Vect2i child = Vect2i(pt.x + sx[i], pt.y + sy[i]);
|
||||
_num_point_examine++;
|
||||
|
||||
if (child.x < 0 || child.y < 0 ||
|
||||
child.x >= _dx || child.y >= _dy)continue;
|
||||
p = _chart + child.y * _dx + child.x;
|
||||
|
||||
|
||||
TypeH addg = _heuristic->getG(pt.x, pt.y, child.x, child.y);
|
||||
TypeH newg = parent->g + addg;
|
||||
|
||||
if (p->used == _is_used_num) {
|
||||
if (!p->is_open)continue;
|
||||
if (p->g <= newg)continue;
|
||||
|
||||
//Удаляем элемент из _open_map
|
||||
TypeH f = p->f();
|
||||
typename type_point_map::iterator cur = _open_map.find(p->f());
|
||||
bool erase = false;
|
||||
while (cur != _open_map.end()) {
|
||||
if ((*cur).first != f)break;
|
||||
if ((*cur).second.x == child.x && (*cur).second.y == child.y) {
|
||||
_open_map.erase(cur);
|
||||
erase = true;
|
||||
break;
|
||||
}
|
||||
_num_find_erase++;
|
||||
cur++;
|
||||
}
|
||||
_num_find_erase++;
|
||||
//assert(erase);
|
||||
if (!erase)
|
||||
continue;
|
||||
}
|
||||
|
||||
p->parent = parent;
|
||||
/*
|
||||
{
|
||||
Vect2i pp=posBy(parent);
|
||||
Vect2i pc=posBy(p);
|
||||
assert(abs(pc.x-pp.x)<=1 &&
|
||||
abs(pc.y-pp.y)<=1);
|
||||
}
|
||||
*/
|
||||
p->g = newg;
|
||||
p->h = _heuristic->getH(child.x, child.y);
|
||||
|
||||
_open_map.insert(typename type_point_map::value_type(p->f(), child));
|
||||
|
||||
p->is_open = true;
|
||||
p->used = _is_used_num;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
template<class Heuristic, class TypeH>
|
||||
void AIAStar<Heuristic, TypeH>::getStatistic(
|
||||
int *p_num_point_examine, int *p_num_find_erase) {
|
||||
if (p_num_point_examine)
|
||||
*p_num_point_examine = _num_point_examine;
|
||||
if (p_num_find_erase)
|
||||
*p_num_find_erase = _num_find_erase;
|
||||
}
|
||||
|
||||
///////////////////////AIAStarGraph/////////////
|
||||
//AIAStarGraph - Так-же поиск пути, но ориентированный на поиск в произвольном графе
|
||||
/*
|
||||
class Node
|
||||
{
|
||||
typedef ... iterator;
|
||||
iterator begin();//Работа со списком связанных с этой Node нод.
|
||||
iterator end();
|
||||
|
||||
void* AIAStarPointer;//Используется в AIAStarGraph
|
||||
};
|
||||
|
||||
class Heuristic
|
||||
{
|
||||
float getH(Node* pos);//Предполагаемые затраты на продвижение из pos1 к окончанию
|
||||
float getG(Node* pos1,Node* pos2);//Затраты на продвижение из pos1 в pos2
|
||||
bool IsEndPoint(Node* pos);//Рекурсия должна окончиться здесь
|
||||
//то есть класс AIAStar позволяет задавать несколько точек окончания поиска пути
|
||||
};
|
||||
*/
|
||||
|
||||
template<class Heuristic, class Node, class TypeH = float>
|
||||
class AIAStarGraph {
|
||||
public:
|
||||
struct OnePoint;
|
||||
typedef Common::MultiMap<TypeH, OnePoint *> type_point_map;
|
||||
|
||||
struct OnePoint {
|
||||
TypeH g;//Затраты на продвижение до этой точки
|
||||
TypeH h;//Предполагаемые затраты на продвижение до финиша
|
||||
int used;
|
||||
OnePoint *parent;
|
||||
bool is_open;
|
||||
|
||||
Node *node;
|
||||
typename type_point_map::iterator self_it;
|
||||
|
||||
inline TypeH f() {
|
||||
return g + h;
|
||||
}
|
||||
};
|
||||
protected:
|
||||
Std::vector<OnePoint> _chart;
|
||||
type_point_map _open_map;
|
||||
|
||||
int _is_used_num;//Если _is_used_num==used, то ячейка используется
|
||||
|
||||
int _num_point_examine;//количество посещённых ячеек
|
||||
int _num_find_erase;//Сколько суммарно искали ячейки для удаления
|
||||
Heuristic *_heuristic;
|
||||
public:
|
||||
AIAStarGraph();
|
||||
~AIAStarGraph();
|
||||
|
||||
//Общее количество узлов. Константа, которая не должна меняться,
|
||||
//пока существует класс, указывающий на неё.
|
||||
void init(Std::vector<Node> &all_node);
|
||||
|
||||
bool findPath(Node *from, Heuristic *h, Std::vector<Node *> &path);
|
||||
void getStatistic(int *num_point_examine, int *num_find_erase);
|
||||
|
||||
//Debug
|
||||
OnePoint *getInternalBuffer() {
|
||||
return _chart;
|
||||
};
|
||||
int getUsedNum() {
|
||||
return _is_used_num;
|
||||
}
|
||||
protected:
|
||||
void clear();
|
||||
inline Node *posBy(OnePoint *p) {
|
||||
return p->node;
|
||||
}
|
||||
};
|
||||
|
||||
template<class Heuristic, class Node, class TypeH>
|
||||
AIAStarGraph<Heuristic, Node, TypeH>::AIAStarGraph() {
|
||||
_heuristic = NULL;
|
||||
_is_used_num = 0;
|
||||
_num_point_examine = 0;
|
||||
_num_find_erase = 0;
|
||||
}
|
||||
|
||||
template<class Heuristic, class Node, class TypeH>
|
||||
void AIAStarGraph<Heuristic, Node, TypeH>::init(Std::vector<Node> &all_node) {
|
||||
int size = all_node.size();
|
||||
_chart.resize(size);
|
||||
|
||||
for (int i = 0; i < size; i++) {
|
||||
OnePoint *c = &_chart[i];
|
||||
c->node = &all_node[i];
|
||||
c->node->AIAStarPointer = (void *)c;
|
||||
}
|
||||
clear();
|
||||
}
|
||||
|
||||
template<class Heuristic, class Node, class TypeH>
|
||||
void AIAStarGraph<Heuristic, Node, TypeH>::clear() {
|
||||
_is_used_num = 0;
|
||||
for (auto &it : _chart) {
|
||||
it.used = 0;
|
||||
}
|
||||
}
|
||||
|
||||
template<class Heuristic, class Node, class TypeH>
|
||||
AIAStarGraph<Heuristic, Node, TypeH>::~AIAStarGraph() {
|
||||
}
|
||||
|
||||
template<class Heuristic, class Node, class TypeH>
|
||||
bool AIAStarGraph<Heuristic, Node, TypeH>::findPath(Node *from, Heuristic *hr, Std::vector<Node *> &path) {
|
||||
_num_point_examine = 0;
|
||||
_num_find_erase = 0;
|
||||
|
||||
_is_used_num++;
|
||||
_open_map.clear();
|
||||
path.clear();
|
||||
if (_is_used_num == 0)
|
||||
clear();//Для того, чтобы вызвалась эта строчка, необходимо гиганское время
|
||||
_heuristic = hr;
|
||||
|
||||
OnePoint *p = (OnePoint *)from->AIAStarPointer;
|
||||
Node *from_node = p->node;
|
||||
p->g = 0;
|
||||
p->h = _heuristic->getH(p->node);
|
||||
p->used = _is_used_num;
|
||||
p->is_open = true;
|
||||
p->parent = NULL;
|
||||
|
||||
p->self_it = _open_map.insert(type_point_map::value_type(p->f(), p));
|
||||
|
||||
while (!_open_map.empty()) {
|
||||
typename type_point_map::iterator low = _open_map.begin();
|
||||
|
||||
OnePoint *parent = low->second;
|
||||
Node *node = parent->node;
|
||||
|
||||
parent->is_open = false;
|
||||
_open_map.erase(low);
|
||||
|
||||
if (_heuristic->IsEndPoint(node)) {
|
||||
//сконструировать путь
|
||||
Node *np;
|
||||
while (parent) {
|
||||
np = PosBy(parent);
|
||||
assert(parent->used == _is_used_num);
|
||||
|
||||
path.push_back(np);
|
||||
parent = parent->parent;
|
||||
}
|
||||
assert(np == from_node);
|
||||
reverse(path.begin(), path.end());
|
||||
return true;
|
||||
}
|
||||
|
||||
//для каждого наследника child узла parent
|
||||
for (auto &it : *node) {
|
||||
Node *cur_node = *it;
|
||||
OnePoint *op = (OnePoint *)cur_node->AIAStarPointer;
|
||||
_num_point_examine++;
|
||||
|
||||
TypeH addg = _heuristic->getG(node, cur_node);
|
||||
TypeH newg = parent->g + addg;
|
||||
|
||||
if (op->used == _is_used_num) {
|
||||
if (!op->is_open)continue;
|
||||
if (op->g <= newg)continue;
|
||||
|
||||
_open_map.erase(op->self_it);
|
||||
_num_find_erase++;
|
||||
}
|
||||
|
||||
op->parent = parent;
|
||||
op->g = newg;
|
||||
op->h = _heuristic->getH(cur_node);
|
||||
|
||||
op->self_it = _open_map.insert(type_point_map::value_type(op->f(), op));
|
||||
|
||||
op->is_open = true;
|
||||
op->used = _is_used_num;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
template<class Heuristic, class Node, class TypeH>
|
||||
void AIAStarGraph<Heuristic, Node, TypeH>::getStatistic(
|
||||
int *p_num_point_examine, int *p_num_find_erase) {
|
||||
if (p_num_point_examine)
|
||||
*p_num_point_examine = _num_point_examine;
|
||||
if (p_num_find_erase)
|
||||
*p_num_find_erase = _num_find_erase;
|
||||
}
|
||||
|
||||
///////////////////////AIFindMaxium/////////////
|
||||
|
||||
/*
|
||||
struct Maps
|
||||
{
|
||||
//Значение чего нибудь в точке (x,y)
|
||||
TypeH get(int x,int y);
|
||||
|
||||
//Дальнейшие поиски можно прекратить
|
||||
bool IsOptiumGood(TypeH optium,int x,int y);
|
||||
};
|
||||
*/
|
||||
|
||||
//Ищет минимальное значение, но не по всей карте
|
||||
template<class Maps>
|
||||
Vect2i AIFindMinium(int x, int y,
|
||||
Maps &maps,
|
||||
int dx, int dy) {
|
||||
typename Maps::TypeH optium = maps.get(x, y);
|
||||
int optiumx = x, optiumy = y;
|
||||
|
||||
int maxi = MAX(MAX(x, dx - x), MAX(y, dy - y));
|
||||
for (int i = 1; i < maxi; i++) {
|
||||
int curx, cury;
|
||||
int xmin = MAX(0, x - i), xmax = MIN(dx - 1, x + i);
|
||||
int ymin = MAX(0, y - i), ymax = MIN(dy - 1, y + i);
|
||||
//up
|
||||
cury = y - i;
|
||||
if (cury >= 0)
|
||||
for (curx = xmin; curx <= xmax; curx++) {
|
||||
typename Maps::TypeH o = maps.get(curx, cury);
|
||||
if (o < optium) {
|
||||
optium = o;
|
||||
optiumx = curx;
|
||||
optiumy = cury;
|
||||
}
|
||||
}
|
||||
|
||||
//down
|
||||
cury = y + i;
|
||||
if (cury < dy)
|
||||
for (curx = xmin; curx <= xmax; curx++) {
|
||||
typename Maps::TypeH o = maps.get(curx, cury);
|
||||
if (o < optium) {
|
||||
optium = o;
|
||||
optiumx = curx;
|
||||
optiumy = cury;
|
||||
}
|
||||
}
|
||||
|
||||
//left
|
||||
curx = x - i;
|
||||
if (curx >= 0)
|
||||
for (cury = ymin; cury <= ymax; cury++) {
|
||||
typename Maps::TypeH o = maps.get(curx, cury);
|
||||
if (o < optium) {
|
||||
optium = o;
|
||||
optiumx = curx;
|
||||
optiumy = cury;
|
||||
}
|
||||
}
|
||||
|
||||
//right
|
||||
curx = x + i;
|
||||
if (curx < dx)
|
||||
for (cury = ymin; cury <= ymax; cury++) {
|
||||
typename Maps::TypeH o = maps.get(curx, cury);
|
||||
if (o < optium) {
|
||||
optium = o;
|
||||
optiumx = curx;
|
||||
optiumy = cury;
|
||||
}
|
||||
}
|
||||
|
||||
if (maps.IsOptiumGood(optium, optiumx, optiumy))
|
||||
break;
|
||||
}
|
||||
|
||||
Vect2i p = {optiumx, optiumy};
|
||||
return p;
|
||||
}
|
||||
} // namespace QDEngine
|
||||
Reference in New Issue
Block a user