mirror of
https://github.com/glest/glest-source.git
synced 2025-02-25 20:22:31 +01:00
671 lines
19 KiB
C++
671 lines
19 KiB
C++
// ==============================================================
|
|
// This file is part of Glest (www.glest.org)
|
|
//
|
|
// Copyright (C) 2009 James McCulloch <silnarm at gmail>
|
|
//
|
|
// You can redistribute this code and/or modify it under
|
|
// the terms of the GNU General Public License as published
|
|
// by the Free Software Foundation; either version 2 of the
|
|
// License, or (at your option) any later version
|
|
// ==============================================================
|
|
|
|
#include <algorithm>
|
|
|
|
#include "cluster_map.h"
|
|
#include "node_pool.h"
|
|
#include "route_planner.h"
|
|
|
|
#include "line.h"
|
|
|
|
using std::min;
|
|
using Shared::Util::line;
|
|
|
|
#define _USE_LINE_PATH_ 1
|
|
|
|
#if DEBUG_RENDERING_ENABLED
|
|
# include "debug_renderer.h"
|
|
#endif
|
|
|
|
namespace Glest { namespace Game {
|
|
|
|
int Edge::numEdges[fieldCount];
|
|
int Transition::numTransitions[fieldCount];
|
|
|
|
void Edge::zeroCounters() {
|
|
for (int f = 0; f < fieldCount; ++f) {
|
|
numEdges[f] = 0;
|
|
}
|
|
}
|
|
|
|
void Transition::zeroCounters() {
|
|
for (int f = 0; f < fieldCount; ++f) {
|
|
numTransitions[f] = 0;
|
|
}
|
|
}
|
|
|
|
ClusterMap::ClusterMap(AnnotatedMap *aMap, Cartographer *carto)
|
|
: carto(carto), aMap(aMap), dirty(false) {
|
|
//_PROFILE_FUNCTION();
|
|
w = aMap->getWidth() / GameConstants::clusterSize;
|
|
h = aMap->getHeight() / GameConstants::clusterSize;
|
|
vertBorders = new ClusterBorder[(w-1)*h];
|
|
horizBorders = new ClusterBorder[w*(h-1)];
|
|
|
|
Edge::zeroCounters();
|
|
Transition::zeroCounters();
|
|
|
|
// init Borders (and hence inter-cluster edges) & evaluate clusters (intra-cluster edges)
|
|
for (int i = h - 1; i >= 0; --i) {
|
|
for (int j = w - 1; j >= 0; --j) {
|
|
Vec2i cluster(j, i);
|
|
initCluster(cluster);
|
|
evalCluster(cluster);
|
|
//g_logger.clusterInit();
|
|
}
|
|
}
|
|
}
|
|
|
|
ClusterMap::~ClusterMap() {
|
|
delete [] vertBorders;
|
|
delete [] horizBorders;
|
|
for (int f = 0; f < fieldCount; ++f) {
|
|
assert(Edge::NumEdges(Field(f)) == 0);
|
|
assert(Transition::NumTransitions(Field(f)) == 0);
|
|
if (Edge::NumEdges(Field(f)) != 0 || Transition::NumTransitions(Field(f)) != 0) {
|
|
throw runtime_error("memory leak");
|
|
}
|
|
}
|
|
}
|
|
|
|
#define LOG(x) {}
|
|
|
|
void ClusterMap::assertValid() {
|
|
bool valid[fieldCount];
|
|
bool inUse[fieldCount];
|
|
int numNodes[fieldCount];
|
|
int numEdges[fieldCount];
|
|
|
|
for (int f = 0; f < fieldCount; ++f) {
|
|
typedef set<const Transition *> TSet;
|
|
TSet tSet;
|
|
|
|
typedef pair<Vec2i, bool> TKey;
|
|
typedef map<TKey, const Transition *> TKMap;
|
|
|
|
TKMap tkMap;
|
|
|
|
valid[f] = true;
|
|
numNodes[f] = 0;
|
|
numEdges[f] = 0;
|
|
inUse[f] = aMap->maxClearance[f] != 0;
|
|
if (f == fAir) inUse[f] = false;
|
|
if (!inUse[f]) {
|
|
continue;
|
|
}
|
|
|
|
// collect all transitions, checking for membership in tSet and tkMap (indicating an error)
|
|
// and filling tSet and tkMap
|
|
for (int i=0; i < (w - 1) * h; ++i) { // vertical borders
|
|
ClusterBorder *b = &vertBorders[i];
|
|
for (int j=0; j < b->transitions[f].n; ++j) {
|
|
const Transition *t = b->transitions[f].transitions[j];
|
|
if (tSet.find(t) != tSet.end()) {
|
|
LOG("single transition on multiple borders.\n");
|
|
valid[f] = false;
|
|
} else {
|
|
tSet.insert(t);
|
|
TKey key(t->nwPos, t->vertical);
|
|
if (tkMap.find(key) != tkMap.end()) {
|
|
LOG("seperate transitions of same orientation on same cell.\n");
|
|
valid[f] = false;
|
|
} else {
|
|
tkMap[key] = t;
|
|
}
|
|
}
|
|
++numNodes[f];
|
|
}
|
|
}
|
|
for (int i=0; i < w * (h - 1); ++i) { // horizontal borders
|
|
ClusterBorder *b = &horizBorders[i];
|
|
for (int j=0; j < b->transitions[f].n; ++j) {
|
|
const Transition *t = b->transitions[f].transitions[j];
|
|
if (tSet.find(t) != tSet.end()) {
|
|
LOG("single transition on multiple borders.\n");
|
|
valid[f] = false;
|
|
} else {
|
|
tSet.insert(t);
|
|
TKey key(t->nwPos, t->vertical);
|
|
if (tkMap.find(key) != tkMap.end()) {
|
|
LOG("seperate transitions of same orientation on same cell.\n");
|
|
valid[f] = false;
|
|
} else {
|
|
tkMap[key] = t;
|
|
}
|
|
}
|
|
++numNodes[f];
|
|
}
|
|
}
|
|
|
|
// with a complete collection, iterate, check all dest transitions
|
|
for (TSet::iterator it = tSet.begin(); it != tSet.end(); ++it) {
|
|
const Edges &edges = (*it)->edges;
|
|
for (Edges::const_iterator eit = edges.begin(); eit != edges.end(); ++eit) {
|
|
TSet::iterator it2 = tSet.find((*eit)->transition());
|
|
if (it2 == tSet.end()) {
|
|
LOG("Invalid edge.\n");
|
|
valid[f] = false;
|
|
} else {
|
|
if (*it == *it2) {
|
|
LOG("self referential transition.\n");
|
|
valid[f] = false;
|
|
}
|
|
}
|
|
++numEdges[f];
|
|
}
|
|
}
|
|
}
|
|
LOG("\nClusterMap::assertValid()");
|
|
LOG("\n=========================\n");
|
|
for (int f = 0; f < fieldCount; ++f) {
|
|
if (!inUse[f]) {
|
|
LOG("Field::" << FieldNames[f] << " not in use.\n");
|
|
} else {
|
|
LOG("Field::" << FieldNames[f] << " in use and " << (!valid[f]? "NOT " : "") << "valid.\n");
|
|
LOG("\t" << numNodes[f] << " transitions inspected.\n");
|
|
LOG("\t" << numEdges[f] << " edges inspected.\n");
|
|
}
|
|
}
|
|
}
|
|
|
|
/** Entrance init helper class */
|
|
class InsideOutIterator {
|
|
private:
|
|
int centre, incr, end;
|
|
bool flip;
|
|
|
|
public:
|
|
InsideOutIterator(int low, int high)
|
|
: flip(false) {
|
|
centre = low + (high - low) / 2;
|
|
incr = 0;
|
|
end = ((high - low) % 2 != 0) ? low - 1 : high + 1;
|
|
}
|
|
|
|
int operator*() const {
|
|
return centre + (flip ? incr : -incr);
|
|
}
|
|
|
|
void operator++() {
|
|
flip = !flip;
|
|
if (flip) ++incr;
|
|
}
|
|
|
|
bool more() {
|
|
return **this != end;
|
|
}
|
|
};
|
|
|
|
void ClusterMap::addBorderTransition(EntranceInfo &info) {
|
|
assert(info.max_clear > 0 && info.startPos != -1 && info.endPos != -1);
|
|
if (info.run < 12) {
|
|
// find central most pos with max clearance
|
|
InsideOutIterator it(info.endPos, info.startPos);
|
|
while (it.more()) {
|
|
if (eClear[info.startPos - *it] == info.max_clear) {
|
|
Transition *t;
|
|
if (info.vert) {
|
|
t = new Transition(Vec2i(info.pos, *it), info.max_clear, true, info.f);
|
|
} else {
|
|
t = new Transition(Vec2i(*it, info.pos), info.max_clear, false, info.f);
|
|
}
|
|
info.cb->transitions[info.f].add(t);
|
|
return;
|
|
}
|
|
++it;
|
|
}
|
|
assert(false);
|
|
} else {
|
|
// look for two, as close as possible to 1/4 and 3/4 accross
|
|
int l1 = info.endPos;
|
|
int h1 = info.endPos + (info.startPos - info.endPos) / 2 - 1;
|
|
int l2 = info.endPos + (info.startPos - info.endPos) / 2;
|
|
int h2 = info.startPos;
|
|
InsideOutIterator it(l1, h1);
|
|
int first_at = -1;
|
|
while (it.more()) {
|
|
if (eClear[info.startPos - *it] == info.max_clear) {
|
|
first_at = *it;
|
|
break;
|
|
}
|
|
++it;
|
|
}
|
|
if (first_at != -1) {
|
|
it = InsideOutIterator(l2, h2);
|
|
int next_at = -1;
|
|
while (it.more()) {
|
|
if (eClear[info.startPos - *it] == info.max_clear) {
|
|
next_at = *it;
|
|
break;
|
|
}
|
|
++it;
|
|
}
|
|
if (next_at != -1) {
|
|
Transition *t1, *t2;
|
|
if (info.vert) {
|
|
t1 = new Transition(Vec2i(info.pos, first_at), info.max_clear, true, info.f);
|
|
t2 = new Transition(Vec2i(info.pos, next_at), info.max_clear, true, info.f);
|
|
} else {
|
|
t1 = new Transition(Vec2i(first_at, info.pos), info.max_clear, false, info.f);
|
|
t2 = new Transition(Vec2i(next_at, info.pos), info.max_clear, false, info.f);
|
|
}
|
|
info.cb->transitions[info.f].add(t1);
|
|
info.cb->transitions[info.f].add(t2);
|
|
return;
|
|
}
|
|
}
|
|
// failed to find two, just add one...
|
|
it = InsideOutIterator(info.endPos, info.startPos);
|
|
while (it.more()) {
|
|
if (eClear[info.startPos - *it] == info.max_clear) {
|
|
Transition *t;
|
|
if (info.vert) {
|
|
t = new Transition(Vec2i(info.pos, *it), info.max_clear, true, info.f);
|
|
} else {
|
|
t = new Transition(Vec2i(*it, info.pos), info.max_clear, false, info.f);
|
|
}
|
|
info.cb->transitions[info.f].add(t);
|
|
return;
|
|
}
|
|
++it;
|
|
}
|
|
assert(false);
|
|
}
|
|
}
|
|
|
|
void ClusterMap::initClusterBorder(const Vec2i &cluster, bool north) {
|
|
//_PROFILE_FUNCTION();
|
|
ClusterBorder *cb = north ? getNorthBorder(cluster) : getWestBorder(cluster);
|
|
EntranceInfo inf;
|
|
inf.cb = cb;
|
|
inf.vert = !north;
|
|
if (cb != &sentinel) {
|
|
int pos = north ? cluster.y * GameConstants::clusterSize - 1
|
|
: cluster.x * GameConstants::clusterSize - 1;
|
|
inf.pos = pos;
|
|
int pos2 = pos + 1;
|
|
bool clear = false; // true while evaluating a Transition, false when obstacle hit
|
|
inf.max_clear = -1; // max clearance seen for current Transition
|
|
inf.startPos = -1; // start position of entrance
|
|
inf.endPos = -1; // end position of entrance
|
|
inf.run = 0; // to count entrance 'width'
|
|
for (int f = 0; f < fieldCount; ++f) {
|
|
if (!aMap->maxClearance[f] || f == fAir) continue;
|
|
|
|
# if DEBUG_RENDERING_ENABLED
|
|
if (f == fLand) {
|
|
for (int i=0; i < cb->transitions[f].n; ++i) {
|
|
getDebugRenderer().getCMOverlay().entranceCells.erase(
|
|
cb->transitions[f].transitions[i]->nwPos
|
|
);
|
|
}
|
|
}
|
|
# endif
|
|
|
|
cb->transitions[f].clear();
|
|
clear = false;
|
|
inf.f = Field(f);
|
|
inf.max_clear = -1;
|
|
for (int i=0; i < GameConstants::clusterSize; ++i) {
|
|
int clear1, clear2;
|
|
if (north) {
|
|
clear1 = aMap->metrics[Vec2i(POS_X,pos)].get(inf.f);
|
|
clear2 = aMap->metrics[Vec2i(POS_X,pos2)].get(inf.f);
|
|
} else {
|
|
clear1 = aMap->metrics[Vec2i(pos, POS_Y)].get(Field(f));
|
|
clear2 = aMap->metrics[Vec2i(pos2, POS_Y)].get(Field(f));
|
|
}
|
|
int local = min(clear1, clear2);
|
|
if (local) {
|
|
if (!clear) {
|
|
clear = true;
|
|
inf.startPos = north ? POS_X : POS_Y;
|
|
}
|
|
eClear[inf.run++] = local;
|
|
inf.endPos = north ? POS_X : POS_Y;
|
|
if (local > inf.max_clear) {
|
|
inf.max_clear = local;
|
|
}
|
|
} else {
|
|
if (clear) {
|
|
addBorderTransition(inf);
|
|
inf.run = 0;
|
|
inf.startPos = inf.endPos = inf.max_clear = -1;
|
|
clear = false;
|
|
}
|
|
}
|
|
} // for i < clusterSize
|
|
if (clear) {
|
|
addBorderTransition(inf);
|
|
inf.run = 0;
|
|
inf.startPos = inf.endPos = inf.max_clear = -1;
|
|
clear = false;
|
|
}
|
|
}// for each Field
|
|
|
|
# if DEBUG_RENDERING_ENABLED
|
|
for (int i=0; i < cb->transitions[fLand].n; ++i) {
|
|
getDebugRenderer().getCMOverlay().entranceCells.insert(
|
|
cb->transitions[fLand].transitions[i]->nwPos
|
|
);
|
|
}
|
|
# endif
|
|
|
|
} // if not sentinel
|
|
}
|
|
|
|
/** function object for line alg. 'visit' */
|
|
struct Visitor {
|
|
vector<Vec2i> &results;
|
|
Visitor(vector<Vec2i> &res) : results(res) {}
|
|
|
|
void operator()(int x, int y) {
|
|
results.push_back(Vec2i(x, y));
|
|
}
|
|
};
|
|
|
|
/** compute path length using midpoint line algorithm, @return infinite if path not possible, else cost */
|
|
float ClusterMap::linePathLength(Field f, int size, const Vec2i &start, const Vec2i &dest) {
|
|
//_PROFILE_FUNCTION();
|
|
if (start == dest) {
|
|
return 0.f;
|
|
}
|
|
vector<Vec2i> linePath;
|
|
Visitor visitor(linePath);
|
|
line(start.x, start.y, dest.x, dest.y, visitor);
|
|
assert(linePath.size() >= 2);
|
|
MoveCost costFunc(f, size, aMap);
|
|
vector<Vec2i>::iterator it = linePath.begin();
|
|
vector<Vec2i>::iterator nIt = it + 1;
|
|
float cost = 0.f;
|
|
while (nIt != linePath.end()) {
|
|
float add = costFunc(*it++, *nIt++);
|
|
if (add != -1.f) {
|
|
cost += add;
|
|
} else {
|
|
return -1.f;
|
|
}
|
|
}
|
|
return cost;
|
|
}
|
|
|
|
/** compute path length using A* (with node limit), @return infinite if path not possible, else cost */
|
|
float ClusterMap::aStarPathLength(Field f, int size, const Vec2i &start, const Vec2i &dest) {
|
|
//_PROFILE_FUNCTION();
|
|
if (start == dest) {
|
|
return 0.f;
|
|
}
|
|
SearchEngine<NodePool> *se = carto->getRoutePlanner()->getSearchEngine();
|
|
MoveCost costFunc(f, size, aMap);
|
|
DiagonalDistance dd(dest);
|
|
se->setNodeLimit(GameConstants::clusterSize * GameConstants::clusterSize);
|
|
se->setStart(start, dd(start));
|
|
PosGoal goal(dest);
|
|
AStarResult res = se->aStar<PosGoal,MoveCost,DiagonalDistance>(goal, costFunc, dd);
|
|
Vec2i goalPos = se->getGoalPos();
|
|
if (res != asrComplete || goalPos != dest) {
|
|
return -1.f;
|
|
}
|
|
return se->getCostTo(goalPos);
|
|
}
|
|
|
|
void ClusterMap::getTransitions(const Vec2i &cluster, Field f, Transitions &t) {
|
|
ClusterBorder *b = getNorthBorder(cluster);
|
|
for (int i=0; i < b->transitions[f].n; ++i) {
|
|
t.push_back(b->transitions[f].transitions[i]);
|
|
}
|
|
b = getEastBorder(cluster);
|
|
for (int i=0; i < b->transitions[f].n; ++i) {
|
|
t.push_back(b->transitions[f].transitions[i]);
|
|
}
|
|
b = getSouthBorder(cluster);
|
|
for (int i=0; i < b->transitions[f].n; ++i) {
|
|
t.push_back(b->transitions[f].transitions[i]);
|
|
}
|
|
b = getWestBorder(cluster);
|
|
for (int i=0; i < b->transitions[f].n; ++i) {
|
|
t.push_back(b->transitions[f].transitions[i]);
|
|
}
|
|
}
|
|
|
|
void ClusterMap::disconnectCluster(const Vec2i &cluster) {
|
|
//cout << "Disconnecting cluster " << cluster << endl;
|
|
for (int f = 0; f < fieldCount; ++f) {
|
|
if (!aMap->maxClearance[f] || f == fAir) continue;
|
|
Transitions t;
|
|
getTransitions(cluster, Field(f), t);
|
|
set<const Transition*> tset;
|
|
for (Transitions::iterator it = t.begin(); it != t.end(); ++it) {
|
|
tset.insert(*it);
|
|
}
|
|
//int del = 0;
|
|
for (Transitions::iterator it = t.begin(); it != t.end(); ++it) {
|
|
Transition *t = const_cast<Transition*>(*it);
|
|
Edges::iterator eit = t->edges.begin();
|
|
while (eit != t->edges.end()) {
|
|
if (tset.find((*eit)->transition()) != tset.end()) {
|
|
delete *eit;
|
|
eit = t->edges.erase(eit);
|
|
//++del;
|
|
} else {
|
|
++eit;
|
|
}
|
|
}
|
|
}
|
|
//cout << "\tDeleted " << del << " edges in Field = " << FieldNames[f] << ".\n";
|
|
}
|
|
|
|
}
|
|
|
|
void ClusterMap::update() {
|
|
//_PROFILE_FUNCTION();
|
|
//cout << "ClusterMap::update()" << endl;
|
|
for (set<Vec2i>::iterator it = dirtyNorthBorders.begin(); it != dirtyNorthBorders.end(); ++it) {
|
|
if (it->y > 0 && it->y < h) {
|
|
dirtyClusters.insert(Vec2i(it->x, it->y));
|
|
dirtyClusters.insert(Vec2i(it->x, it->y - 1));
|
|
}
|
|
}
|
|
for (set<Vec2i>::iterator it = dirtyWestBorders.begin(); it != dirtyWestBorders.end(); ++it) {
|
|
if (it->x > 0 && it->x < w) {
|
|
dirtyClusters.insert(Vec2i(it->x, it->y));
|
|
dirtyClusters.insert(Vec2i(it->x - 1, it->y));
|
|
}
|
|
}
|
|
for (set<Vec2i>::iterator it = dirtyClusters.begin(); it != dirtyClusters.end(); ++it) {
|
|
//cout << "cluster " << *it << " dirty." << endl;
|
|
disconnectCluster(*it);
|
|
}
|
|
for (set<Vec2i>::iterator it = dirtyNorthBorders.begin(); it != dirtyNorthBorders.end(); ++it) {
|
|
//cout << "cluster " << *it << " north border dirty." << endl;
|
|
initClusterBorder(*it, true);
|
|
}
|
|
for (set<Vec2i>::iterator it = dirtyWestBorders.begin(); it != dirtyWestBorders.end(); ++it) {
|
|
//cout << "cluster " << *it << " west border dirty." << endl;
|
|
initClusterBorder(*it, false);
|
|
}
|
|
for (set<Vec2i>::iterator it = dirtyClusters.begin(); it != dirtyClusters.end(); ++it) {
|
|
evalCluster(*it);
|
|
}
|
|
|
|
dirtyClusters.clear();
|
|
dirtyNorthBorders.clear();
|
|
dirtyWestBorders.clear();
|
|
dirty = false;
|
|
}
|
|
|
|
|
|
/** compute intra-cluster path lengths */
|
|
void ClusterMap::evalCluster(const Vec2i &cluster) {
|
|
//_PROFILE_FUNCTION();
|
|
int linePathSuccess = 0, linePathFail = 0;
|
|
SearchEngine<NodePool> *se = carto->getRoutePlanner()->getSearchEngine();
|
|
se->getNeighbourFunc().setSearchCluster(cluster);
|
|
Transitions transitions;
|
|
for (int f = 0; f < fieldCount; ++f) {
|
|
if (!aMap->maxClearance[f] || f == fAir) continue;
|
|
transitions.clear();
|
|
getTransitions(cluster, Field(f), transitions);
|
|
Transitions::iterator it = transitions.begin();
|
|
for ( ; it != transitions.end(); ++it) { // foreach transition
|
|
Transition *t = const_cast<Transition*>(*it);
|
|
Vec2i start = t->nwPos;
|
|
Transitions::iterator it2 = transitions.begin();
|
|
for ( ; it2 != transitions.end(); ++it2) { // foreach other transition
|
|
const Transition* &t2 = *it2;
|
|
if (t == t2) continue;
|
|
Vec2i dest = t2->nwPos;
|
|
# if _USE_LINE_PATH_
|
|
float cost = linePathLength(Field(f), 1, start, dest);
|
|
if (cost == -1.f) {
|
|
cost = aStarPathLength(Field(f), 1, start, dest);
|
|
}
|
|
# else
|
|
float cost = aStarPathLength(Field(f), 1, start, dest);
|
|
# endif
|
|
if (cost == -1.f) continue;
|
|
Edge *e = new Edge(t2, Field(f));
|
|
t->edges.push_back(e);
|
|
e->addWeight(cost);
|
|
int size = 2;
|
|
int maxClear = t->clearance > t2->clearance ? t2->clearance : t->clearance;
|
|
while (size <= maxClear) {
|
|
# if _USE_LINE_PATH_
|
|
cost = linePathLength(Field(f), 1, start, dest);
|
|
if (cost == -1.f) {
|
|
cost = aStarPathLength(Field(f), size, start, dest);
|
|
}
|
|
# else
|
|
float cost = aStarPathLength(Field(f), size, start, dest);
|
|
# endif
|
|
if (cost == -1.f) {
|
|
break;
|
|
}
|
|
e->addWeight(cost);
|
|
assert(size == e->maxClear());
|
|
++size;
|
|
}
|
|
} // for each other transition
|
|
} // for each transition
|
|
} // for each Field
|
|
se->getNeighbourFunc().setSearchSpace(ssCellMap);
|
|
}
|
|
|
|
// ========================================================
|
|
// class TransitionNodeStorage
|
|
// ========================================================
|
|
|
|
TransitionAStarNode* TransitionNodeStore::getNode() {
|
|
if (nodeCount == size) {
|
|
//assert(false);
|
|
return NULL;
|
|
}
|
|
return &stock[nodeCount++];
|
|
}
|
|
|
|
void TransitionNodeStore::insertIntoOpen(TransitionAStarNode *node) {
|
|
if (openList.empty()) {
|
|
openList.push_front(node);
|
|
return;
|
|
}
|
|
list<TransitionAStarNode*>::iterator it = openList.begin();
|
|
while (it != openList.end() && (*it)->est() <= node->est()) {
|
|
++it;
|
|
}
|
|
openList.insert(it, node);
|
|
}
|
|
|
|
bool TransitionNodeStore::assertOpen() {
|
|
if (openList.size() < 2) {
|
|
return true;
|
|
}
|
|
set<const Transition*> seen;
|
|
list<TransitionAStarNode*>::iterator it1, it2 = openList.begin();
|
|
it1 = it2;
|
|
seen.insert((*it1)->pos);
|
|
for (++it2; it2 != openList.end(); ++it2) {
|
|
if (seen.find((*it2)->pos) != seen.end()) {
|
|
LOG("open list has cycle... that's bad.");
|
|
return false;
|
|
}
|
|
seen.insert((*it2)->pos);
|
|
if ((*it1)->est() > (*it2)->est() + 0.0001f) { // stupid inaccurate fp
|
|
LOG("Open list corrupt: it1.est() == " << (*it1)->est()
|
|
<< " > it2.est() == " << (*it2)->est());
|
|
return false;
|
|
}
|
|
}
|
|
set<const Transition*>::iterator it = open.begin();
|
|
for ( ; it != open.end(); ++it) {
|
|
if (seen.find(*it) == seen.end()) {
|
|
LOG("node marked open not on open list.");
|
|
return false;
|
|
}
|
|
}
|
|
it = seen.begin();
|
|
for ( ; it != seen.end(); ++it) {
|
|
if (open.find(*it) == open.end()) {
|
|
LOG("node on open list not marked open.");
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
Transition* TransitionNodeStore::getBestSeen() {
|
|
assert(false);
|
|
return NULL;
|
|
}
|
|
|
|
bool TransitionNodeStore::setOpen(const Transition* pos, const Transition* prev, float h, float d) {
|
|
assert(open.find(pos) == open.end());
|
|
assert(closed.find(pos) == closed.end());
|
|
|
|
TransitionAStarNode *node = getNode();
|
|
if (!node) return false;
|
|
node->pos = pos;
|
|
node->prev = prev;
|
|
node->distToHere = d;
|
|
node->heuristic = h;
|
|
open.insert(pos);
|
|
insertIntoOpen(node);
|
|
listed[pos] = node;
|
|
|
|
return true;
|
|
}
|
|
|
|
void TransitionNodeStore::updateOpen(const Transition* pos, const Transition* &prev, const float cost) {
|
|
assert(open.find(pos) != open.end());
|
|
assert(closed.find(prev) != closed.end());
|
|
|
|
TransitionAStarNode *prevNode = listed[prev];
|
|
TransitionAStarNode *posNode = listed[pos];
|
|
if (prevNode->distToHere + cost < posNode->distToHere) {
|
|
openList.remove(posNode);
|
|
posNode->prev = prev;
|
|
posNode->distToHere = prevNode->distToHere + cost;
|
|
insertIntoOpen(posNode);
|
|
}
|
|
}
|
|
|
|
const Transition* TransitionNodeStore::getBestCandidate() {
|
|
if (openList.empty()) return NULL;
|
|
TransitionAStarNode *node = openList.front();
|
|
const Transition *best = node->pos;
|
|
openList.pop_front();
|
|
open.erase(open.find(best));
|
|
closed.insert(best);
|
|
return best;
|
|
}
|
|
|
|
}}
|