* fix (work-around) for apparent MSVC bug, in AStarNode::operator<()

* removed some unused pathfinder stuff, and const-ified some other stuff
 * RoutePlanner is used in scenarios (for testing purposes...)
This commit is contained in:
James McCulloch 2010-08-08 04:43:24 +00:00
parent d280eefcce
commit 3d482ca366
12 changed files with 61 additions and 673 deletions

View File

@ -12,7 +12,6 @@
#include "cartographer.h"
#include "game_constants.h"
#include "route_planner.h"
#include "node_map.h"
#include "pos_iterator.h"
@ -45,14 +44,6 @@ Cartographer::Cartographer(World *world)
int w = cellMap->getW(), h = cellMap->getH();
routePlanner = world->getRoutePlanner();
nodeMap = new NodeMap(cellMap);
GridNeighbours gNeighbours(w, h);
nmSearchEngine = new SearchEngine<NodeMap, GridNeighbours>(gNeighbours, nodeMap, true);
nmSearchEngine->setStorage(nodeMap);
nmSearchEngine->setInvalidKey(Vec2i(-1));
nmSearchEngine->getNeighbourFunc().setSearchSpace(ssCellMap);
masterMap = new AnnotatedMap(world);
clusterMap = new ClusterMap(masterMap, this);
@ -104,10 +95,8 @@ Cartographer::Cartographer(World *world)
/** Destruct */
Cartographer::~Cartographer() {
delete nodeMap;
delete masterMap;
delete clusterMap;
delete nmSearchEngine;
// Goal Maps
deleteMapValues(resourceMaps.begin(), resourceMaps.end());

View File

@ -15,7 +15,6 @@
#include "game_constants.h"
#include "influence_map.h"
#include "annotated_map.h"
#include "node_map.h"
#include "cluster_map.h"
#include "world.h"
@ -106,10 +105,6 @@ private:
StoreMaps storeMaps; /**< goal maps for resource stores */
SiteMaps siteMaps; /**< goal maps for building sites */
// A* stuff
NodeMap *nodeMap;
SearchEngine<NodeMap,GridNeighbours> *nmSearchEngine;
World *world;
Map *cellMap;
RoutePlanner *routePlanner;
@ -135,7 +130,6 @@ public:
Cartographer(World *world);
virtual ~Cartographer();
SearchEngine<NodeMap,GridNeighbours>* getSearchEngine() { return nmSearchEngine; }
RoutePlanner* getRoutePlanner() { return routePlanner; }
/** Update the annotated maps when an obstacle has been added or removed from the map.

View File

@ -1,291 +0,0 @@
// ==============================================================
// 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 "node_map.h"
#include "annotated_map.h" // iterator typedefs
namespace Glest { namespace Game {
/** Construct a NodeMap */
NodeMap::NodeMap(Map *map)
: cellMap(map)
, nodeLimit(-1)
, searchCounter(1)
, nodeCount(0)
, openTop(-1) {
invalidPos.x = invalidPos.y = 65535;
assert(!invalidPos.valid());
bestH = invalidPos;
stride = cellMap->getW();
nodeMap.init(cellMap->getW(), cellMap->getH());
}
/** resets the NodeMap for use */
void NodeMap::reset() {
bestH = invalidPos;
searchCounter += 2;
nodeLimit = -1;
nodeCount = 0;
openTop = Vec2i(-1);
#if _GAE_DEBUG_EDITION_
listedNodes.clear();
#endif
}
/** get the best candidate from the open list, and close it.
* @return the lowest estimate node from the open list, or -1,-1 if open list empty */
Vec2i NodeMap::getBestCandidate() {
if (openTop.x < 0) {
return Vec2i(-1); // empty
}
assert(nodeMap[openTop].mark == searchCounter);
nodeMap[openTop].mark++; // set pos closed
Vec2i ret = openTop;
// set new openTop...
if (nodeMap[openTop].nextOpen.valid()) {
openTop = nodeMap[openTop].nextOpen;
} else {
openTop.x = -1;
}
return ret;
}
/** marks an unvisited position as open
* @param pos the position to open
* @param prev the best known path to pos is from
* @param h the heuristic for pos
* @param d the costSoFar for pos
* @return true if added, false if node limit reached
*/
bool NodeMap::setOpen(const Vec2i &pos, const Vec2i &prev, float h, float d) {
assert(nodeMap[pos].mark < searchCounter);
if (nodeCount == nodeLimit) {
return false;
}
nodeMap[pos].prevNode = prev;
nodeMap[pos].mark = searchCounter;
nodeMap[pos].heuristic = h;
nodeMap[pos].distToHere = d;
float est = h + d;
nodeCount ++;
# if _GAE_DEBUG_EDITION_
listedNodes.push_back ( pos );
# endif
if (!bestH.valid() || nodeMap[pos].heuristic < nodeMap[bestH].heuristic) {
bestH = pos;
}
if (openTop.x == -1) { // top
openTop = pos;
nodeMap[pos].nextOpen = invalidPos;
return true;
}
PackedPos prevOpen, looking = openTop;
// find insert spot...
while (true) {
assert(looking.x != 255);
assert(isOpen(looking));
if (est < nodeMap[looking].estimate()) {
// pos better than looking, insert before 'looking'
nodeMap[pos].nextOpen = looking;
if (openTop == looking) {
openTop = pos;
} else {
assert(nodeMap[prevOpen].nextOpen == looking);
assert(prevOpen.valid());
nodeMap[prevOpen].nextOpen = pos;
}
break;
} else { // est >= nodeMap[looking].estimate()
if (nodeMap[looking].nextOpen.valid()) {
prevOpen = looking;
looking = nodeMap[looking].nextOpen;
} else { // end of list
// insert after nodeMap[looking], set nextOpen - 1
nodeMap[looking].nextOpen = pos;
nodeMap[pos].nextOpen = invalidPos;
break;
}
}
} // while
return true;
}
/** conditionally update a node on the open list. Tests if a path through a new nieghbour
* is better than the existing known best path to pos, updates if so.
* @param pos the open postion to test
* @param prev the new path from
* @param d the distance to here through prev
*/
void NodeMap::updateOpen(const Vec2i &pos, const Vec2i &prev, const float d) {
const float dist = nodeMap[prev].distToHere + d;
if (dist < nodeMap[pos].distToHere) {
//LOG ("Updating open node.");
nodeMap[pos].distToHere = dist;
nodeMap[pos].prevNode = prev;
const float est = nodeMap[pos].estimate();
if (pos == openTop) { // is top of open list anyway
//LOG("was already top");
return;
}
PackedPos oldNext = nodeMap[pos].nextOpen;
if (est < nodeMap[openTop].estimate()) {
nodeMap[pos].nextOpen = openTop;
openTop = pos;
PackedPos ptmp, tmp = nodeMap[pos].nextOpen;
while (pos != tmp) {
assert(nodeMap[tmp].nextOpen.valid());
ptmp = tmp;
tmp = nodeMap[tmp].nextOpen;
}
nodeMap[ptmp].nextOpen = oldNext;
//LOG ( "moved to top" );
return;
}
PackedPos ptmp = openTop, tmp = nodeMap[openTop].nextOpen;
while (true) {
if (pos == tmp) { // no move needed
//LOG("was not moved");
return;
}
if (est < nodeMap[tmp].estimate()) {
nodeMap[pos].nextOpen = tmp;
nodeMap[ptmp].nextOpen = pos;
while (pos != tmp) {
assert(nodeMap[tmp].nextOpen.valid());
ptmp = tmp;
tmp = nodeMap[tmp].nextOpen;
}
//LOG("was moved up");
nodeMap[ptmp].nextOpen = oldNext;
return;
}
ptmp = tmp;
assert(nodeMap[tmp].nextOpen.valid());
tmp = nodeMap[tmp].nextOpen;
}
throw runtime_error("SearchMap::updateOpen() called with non-open position");
}
}
#define LOG(x) {} // {std::cout << x << endl;}
void NodeMap::logOpen() {
if (openTop == Vec2i(-1)) {
LOG("Open list is empty.");
return;
}
static char buffer[4096];
char *ptr = buffer;
PackedPos p = openTop;
while (p.valid()) {
ptr += sprintf(ptr, "%d,%d", p.x, p.y);
if (nodeMap[p].nextOpen.valid()) {
ptr += sprintf(ptr, " => ");
if (ptr - buffer > 4000) {
sprintf(ptr, " => plus more . . .");
break;
}
}
p = nodeMap[p].nextOpen;
}
LOG(buffer);
}
bool NodeMap::assertOpen() {
PackedPos cp;
set<Vec2i> seen;
if (openTop.x == -1) {
return true;
}
// iterate over list, build 'seen' set, checking nextOpen is not there already
cp = openTop;
while (cp.valid()) {
seen.insert(cp);
if (seen.find(nodeMap[cp].nextOpen) != seen.end()) {
LOG("BIG TIME ERROR: open list is cyclic.");
return false;
}
cp = nodeMap[cp].nextOpen;
}
// scan entire grid, check that all nodes marked open are indeed on the open list...
set<Vec2i> valid;
for (int y=0; y < cellMap->getH(); ++y) {
for (int x=0; x < cellMap->getW(); ++x) {
Vec2i pos(x, y);
if (isOpen(pos)) {
if (seen.find(pos) == seen.end()) {
LOG("ERROR: open list missing open node, or non open node claiming to be open.");
return false;
}
valid.insert(pos);
}
}
}
// ... and that all nodes on the list are marked open
for (set<Vec2i>::iterator it = seen.begin(); it != seen.end(); ++it) {
if (valid.find(*it) == valid.end()) {
LOG("ERROR: node on open list was not marked open");
return false;
}
}
return true;
}
bool NodeMap::assertValidPath(list<Vec2i> &path) {
if (path.size() < 2) return true;
VLIt it = path.begin();
Vec2i prevPos = *it;
for (++it; it != path.end(); ++it) {
if (prevPos.dist(*it) < 0.99f || prevPos.dist(*it) > 1.42f) {
return false;
}
prevPos = *it;
}
return true;
}
#if _GAE_DEBUG_EDITION_
/*
list<pair<Vec2i,uint32>>* SearchMap::getLocalAnnotations() {
list<pair<Vec2i,uint32>> *ret = new list<pair<Vec2i,uint32>>();
for ( map<Vec2i,uint32>::iterator it = localAnnt.begin(); it != localAnnt.end(); ++ it ) {
ret->push_back(pair<Vec2i,uint32>(it->first,nodeMap[it->first].getClearance(Field::LAND)));
}
return ret;
}
*/
list<Vec2i>* NodeMap::getOpenNodes() {
list<Vec2i> *ret = new list<Vec2i>();
list<Vec2i>::iterator it = listedNodes.begin();
for ( ; it != listedNodes.end(); ++it ) {
if ( nodeMap[*it].mark == searchCounter ) ret->push_back(*it);
}
return ret;
}
list<Vec2i>* NodeMap::getClosedNodes() {
list<Vec2i> *ret = new list<Vec2i>();
list<Vec2i>::iterator it = listedNodes.begin();
for ( ; it != listedNodes.end(); ++it ) {
if ( nodeMap[*it].mark == searchCounter + 1 ) ret->push_back(*it);
}
return ret;
}
#endif // defined ( _GAE_DEBUG_EDITION_ )
}}

View File

@ -1,185 +0,0 @@
// ==============================================================
// 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
// ==============================================================
#ifndef _GLEST_GAME_ASTAR_NODE_MAP_H_
#define _GLEST_GAME_ASTAR_NODE_MAP_H_
#include "game_constants.h"
#include "vec.h"
#include "world.h"
namespace Glest { namespace Game {
using Shared::Graphics::Vec2i;
using Shared::Platform::uint16;
using Shared::Platform::uint32;
#define MAX_MAP_COORD_BITS 16
#define MAX_MAP_COORD ((1<<MAX_MAP_COORD_BITS)-1)
#pragma pack(push, 1)
/** A bit packed position (Vec2i) */
struct PackedPos {
/** Construct a PackedPos [0,0] */
PackedPos() : x(0), y(0) {}
/** Construct a PackedPos [x,y] */
PackedPos(int x, int y) : x(x), y(y) {}
/** Construct a PackedPos [pos.x, pos.y] */
PackedPos(Vec2i pos) { *this = pos; }
/** Assign from Vec2i */
PackedPos& operator=(Vec2i pos) {
assert(pos.x <= MAX_MAP_COORD && pos.y <= MAX_MAP_COORD);
if (pos.x < 0 || pos.y < 0) {
x = MAX_MAP_COORD; y = MAX_MAP_COORD; // invalid
} else {
x = pos.x; y = pos.y;
}
return *this;
}
/** this packed pos as Vec2i */
operator Vec2i() { return Vec2i(x, y); }
/** Comparision operator */
bool operator==(PackedPos &that) { return x == that.x && y == that.y; }
/** is this position valid */
bool valid() {
if ((x == 65535) || (y == 65535)) {
return false;
}
return true;
}
// max == MAX_MAP_COORD,
// MAX_MAP_COORD, MAX_MAP_COORD is considered 'invalid', this is ok still on a
// MAX_MAP_COORD * MAX_MAP_COORD map in Glest, because the far east and south 'tiles'
// (2 cells) are not valid either.
uint16 x : MAX_MAP_COORD_BITS; /**< x coordinate */
uint16 y : MAX_MAP_COORD_BITS; /**< y coordinate */
};
/** The cell structure for the node map. Stores all the usual A* node information plus
* information on status (unvisited/open/closed) and an 'embedded' linked list for the
* open set. */
struct NodeMapCell {
/** <p>Node status for this search,</p>
* <ul><li>mark < NodeMap::searchCounter => unvisited</li>
* <li>mark == NodeMap::searchCounter => open</li>
* <li>mark == NodeMap::searchCounter + 1 => closed</li></ul> */
uint32 mark;
/** best route to here is from, valid only if this node is closed */
PackedPos prevNode;
/** 'next' node in open list, valid only if this node is open */
PackedPos nextOpen;
/** heuristic from this cell, valid only if node visited */
float heuristic;
/** cost to this cell, valid only if node visited */
float distToHere;
/** Construct NodeMapCell */
NodeMapCell() { memset(this, 0, sizeof(*this)); }
/** the estimate function, costToHere + heuristic */
float estimate() { return heuristic + distToHere; }
};
#pragma pack(pop)
/** Wrapper for the NodeMapCell array */
class NodeMapCellArray {
private:
NodeMapCell *array;
int stride;
public:
NodeMapCellArray() { array = NULL; }
~NodeMapCellArray() { delete [] array; }
void init(int w, int h) { delete [] array; array = new NodeMapCell[w * h]; stride = w; }
/** index by Vec2i */
NodeMapCell& operator[] (const Vec2i &pos) { return array[pos.y * stride + pos.x]; }
/** index by PackedPos */
NodeMapCell& operator[] (const PackedPos pos) { return array[pos.y * stride + pos.x]; }
};
/** A NodeStorage (template interface) compliant NodeMap. Keeps a store of nodes the size of
* the map, and embeds the open and cosed list within. Uses some memory, but goes fast.
*/
class NodeMap {
public:
NodeMap(Map *map);
// NodeStorage template interface
//
void reset();
/** set a maximum number of nodes to expand */
void setMaxNodes(int limit) { nodeLimit = limit > 0 ? limit : -1; }
/** is the node at pos open */
bool isOpen(const Vec2i &pos) { return nodeMap[pos].mark == searchCounter; }
/** is the node at pos closed */
bool isClosed(const Vec2i &pos) { return nodeMap[pos].mark == searchCounter + 1; }
bool setOpen(const Vec2i &pos, const Vec2i &prev, float h, float d);
void updateOpen(const Vec2i &pos, const Vec2i &prev, const float cost);
Vec2i getBestCandidate();
/** get the best heuristic node seen this search */
Vec2i getBestSeen() { return bestH.valid() ? Vec2i(bestH) : Vec2i(-1); }
/** get the heuristic of the node at pos [known to be visited] */
float getHeuristicAt(const Vec2i &pos) { return nodeMap[pos].heuristic; }
/** get the cost to the node at pos [known to be visited] */
float getCostTo(const Vec2i &pos) { return nodeMap[pos].distToHere; }
/** get the estimate for the node at pos [known to be visited] */
float getEstimateFor(const Vec2i &pos) { return nodeMap[pos].estimate(); }
/** get the best path to the node at pos [known to be visited] */
Vec2i getBestTo(const Vec2i &pos) {
return nodeMap[pos].prevNode.valid() ? Vec2i(nodeMap[pos].prevNode) : Vec2i(-1);
}
private:
/** The array of nodes */
NodeMapCellArray nodeMap;
Map *cellMap;
/** The stride of the array */
int stride;
/** The current node expansion limit */
int nodeLimit;
/** A counter, with NodeMapCell::mark determines validity of nodes in current search */
uint32 searchCounter,
/** Number of nodes expanded this/last search */
nodeCount;
/** An invalid PackedPos */
PackedPos invalidPos;
/** The lowest heuristic node seen this/last search */
PackedPos bestH;
/** The top of the open list is at */
Vec2i openTop;
/** Debug */
bool assertValidPath(list<Vec2i> &path);
/** Debug */
bool assertOpen();
/** Debug */
void logOpen();
#ifdef _GAE_DEBUG_EDITION_
public:
list<Vec2i>* getOpenNodes ();
list<Vec2i>* getClosedNodes ();
list<Vec2i> listedNodes;
#endif
};
}}
#endif

View File

@ -94,9 +94,10 @@ void NodePool::addOpenNode(AStarNode *node) {
* is better than the existing known best path to pos, updates if so.
* @param pos the open postion to test
* @param prev the new path from
* @param d the distance to here through prev */
* @param cost the cost to here from prev */
void NodePool::updateOpen(const Vec2i &pos, const Vec2i &prev, const float cost) {
//assert(isClosed(prev));
assert(isClosed(prev));
assert(isOpen(pos));
AStarNode *posNode, *prevNode;
posNode = markerArray.get(pos);
prevNode = markerArray.get(prev);

View File

@ -41,13 +41,13 @@ struct PosOff { /**< A bit packed position (Vec2i) and offset (direction) pai
assert(pos.x <= 8191 && pos.y <= 8191);
x = pos.x; y = pos.y; return *this;
}
bool operator==(PosOff &p)
{ return x == p.x && y == p.y; } /**< compare position components only */
Vec2i getPos() { return Vec2i(x, y); } /**< this packed pos as Vec2i */
Vec2i getPrev() { return Vec2i(x + ox, y + oy); } /**< return pos + offset */
Vec2i getOffset() { return Vec2i(ox, oy); } /**< return offset */
bool hasOffset() { return ox || oy; } /**< has an offset */
bool valid() { return x >= 0 && y >= 0; } /**< is this position valid */
bool operator==(PosOff &p) const
{ return x == p.x && y == p.y; } /**< compare position components only */
Vec2i getPos() const { return Vec2i(x, y); } /**< this packed pos as Vec2i */
Vec2i getPrev() const { return Vec2i(x + ox, y + oy); } /**< return pos + offset */
Vec2i getOffset() const { return Vec2i(ox, oy); } /**< return offset */
bool hasOffset() const { return ox || oy; } /**< has an offset */
bool valid() const { return x >= 0 && y >= 0; } /**< is this position valid */
int32 x : 14; /**< x coordinate */
int32 y : 14; /**< y coordinate */
@ -65,24 +65,25 @@ struct AStarNode { /**< A node structure for A* with NodePool */
float heuristic; /**< estimate of distance to goal */
float distToHere; /**< cost from origin to this node */
float est() const { return distToHere + heuristic;} /**< estimate, costToHere + heuristic */
Vec2i pos() { return posOff.getPos(); } /**< position of this node */
Vec2i prev() { return posOff.getPrev(); } /**< best path to this node is from */
bool hasPrev() { return posOff.hasOffset(); } /**< has valid previous 'pointer' */
float est() const { return distToHere + heuristic;} /**< estimate, costToHere + heuristic */
Vec2i pos() const { return posOff.getPos(); } /**< position of this node */
Vec2i prev() const { return posOff.getPrev(); } /**< best path to this node is from */
bool hasPrev() const{ return posOff.hasOffset(); } /**< has valid previous 'pointer' */
int32 heap_ndx;
void setHeapIndex(int ndx) { heap_ndx = ndx; }
int getHeapIndex() const { return heap_ndx; }
bool operator<(const AStarNode &that) const {
const float diff = (distToHere + heuristic) - (that.distToHere + that.heuristic);
if (diff < 0) return true;
else if (diff > 0) return false;
const float diff = est() - that.est();
if (diff < 0.f) return true;
if (diff > 0.f) return false;
// tie, prefer closer to goal...
if (heuristic < that.heuristic) return true;
if (heuristic > that.heuristic) return false;
const float h_diff = heuristic - that.heuristic;
if (h_diff < 0.f) return true;
if (h_diff > 0.f) return false;
// still tied... just distinguish them somehow...
return this < &that;
return pos() < that.pos();
}
}; // == 128 bits (16 bytes)
#pragma pack(pop)

View File

@ -847,16 +847,16 @@ TravelState RoutePlanner::customGoalSearch(PMap1Goal &goal, Unit *unit, const Ve
UnitPath &path = *advPath;
WaypointPath &wpPath = *unit->getWaypointPath();
const Vec2i &start = unit->getPos();
// setup search
MoveCost moveCost(unit->getCurrField(), unit->getType()->getSize(), world->getCartographer()->getMasterMap());
AnnotatedMap *aMap = world->getCartographer()->getMasterMap();
MoveCost moveCost(unit->getCurrField(), unit->getType()->getSize(), aMap);
DiagonalDistance heuristic(target);
nsgSearchEngine->setNodeLimit(512);
nsgSearchEngine->setStart(start, heuristic(start));
AStarResult r;
AnnotatedMap *aMap = world->getCartographer()->getMasterMap();
aMap->annotateLocal(unit);
r = nsgSearchEngine->aStar(goal, moveCost, heuristic);
AStarResult r = nsgSearchEngine->aStar(goal, moveCost, heuristic);
aMap->clearLocalAnnotations(unit);
PF_TRACE();

View File

@ -212,6 +212,39 @@ public:
}; // class RoutePlanner
/** Diaginal Distance Heuristic */
class DiagonalDistance {
public:
DiagonalDistance(const Vec2i &target) : target(target) {}
/** search target */
Vec2i target;
/** The heuristic function. @param pos the position to calculate the heuristic for
* @return an estimate of the cost to target */
float operator()(const Vec2i &pos) const {
float dx = (float)abs(pos.x - target.x),
dy = (float)abs(pos.y - target.y);
float diag = dx < dy ? dx : dy;
float straight = dx + dy - 2 * diag;
return 1.4 * diag + straight;
}
};
/** Goal function for 'normal' search */
class PosGoal {
private:
Vec2i target; /**< search target */
public:
PosGoal(const Vec2i &target) : target(target) {}
/** The goal function @param pos position to test
* @param costSoFar the cost of the shortest path to pos
* @return true if pos is target, else false */
bool operator()(const Vec2i &pos, const float costSoFar) const {
return pos == target;
}
};
//TODO: put these somewhere sensible
class TransitionHeuristic {
DiagonalDistance dd;

View File

@ -24,8 +24,6 @@ using Shared::Graphics::Vec2i;
static const float SQRT2 = Shared::Graphics::sqrt2;
#include "search_functions.inl"
enum OrdinalDir {
odNorth, odNorthEast, odEast, odSouthEast, odSouth, odSouthWest, odWest, odNorthWest, odCount
};

View File

@ -1,155 +0,0 @@
// ==============================================================
// 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
// ==============================================================
//
// search_functions.inl
//
// INLINE FILE, do not place any silly namespaces or what-not in here, this
// is a part of search_engine.h
//
/** Goal function for 'normal' search */
class PosGoal {
private:
Vec2i target; /**< search target */
public:
PosGoal(const Vec2i &target) : target(target) {}
/** The goal function @param pos position to test
* @param costSoFar the cost of the shortest path to pos
* @return true if pos is target, else false */
bool operator()(const Vec2i &pos, const float costSoFar) const {
return pos == target;
}
};
/** Goal function for 'get within x of' searches */
class RangeGoal {
private:
Vec2i target; /**< search target */
float range; /**< range to get within */
public:
RangeGoal(const Vec2i &target, float range) : target(target), range(range) {}
/** The goal function @param pos position to test
* @param costSoFar the cost of the shortest path to pos
* @return true if pos is within range of target, else false */
bool operator()(const Vec2i &pos, const float costSoFar) const {
return pos.dist(target) <= range;
}
};
/** The 'No Goal' function. Just returns false. Use with care! Use Cost function to control termination
* by exhausting the open list. */
class NoGoal {
public:
NoGoal(){}
/** The goal function @param pos the position to ignore
* @param costSoFar the cost of the shortest path to pos
* @return false */
bool operator()(const Vec2i &pos, const float costSoFar) const {
return false;
}
};
/** A uniform cost function */
class UniformCost {
private:
float cost; /**< The uniform cost to return */
public:
UniformCost(float cost) : cost(cost) {}
/** The cost function @param p1 position 1 @param p2 position 2 ('adjacent' p1)
* @return cost */
float operator()(const Vec2i &p1, const Vec2i &p2) const { return cost; }
};
/** distance cost, no obstacle checks */
class DistanceCost {
public:
DistanceCost(){}
/** The cost function @param p1 position 1 @param p2 position 2 ('adjacent' p1)
* @return 1.0 if p1 and p2 are 'in line', else SQRT2 */
float operator()(const Vec2i &p1, const Vec2i &p2) const {
assert ( p1.dist(p2) < 1.5 );
if ( p1.x != p2.x && p1.y != p2.y ) {
return SQRT2;
}
return 1.0f;
}
};
/** Diaginal Distance Heuristic */
class DiagonalDistance {
public:
DiagonalDistance(const Vec2i &target) : target(target) {}
/** search target */
Vec2i target;
/** The heuristic function. @param pos the position to calculate the heuristic for
* @return an estimate of the cost to target */
float operator()(const Vec2i &pos) const {
float dx = (float)abs(pos.x - target.x),
dy = (float)abs(pos.y - target.y);
float diag = dx < dy ? dx : dy;
float straight = dx + dy - 2 * diag;
return 1.4 * diag + straight;
}
};
/** Diagonal Distance Overestimating Heuristic */
class OverEstimate {
private:
Vec2i target; /**< search target */
public:
OverEstimate(const Vec2i &target) : target(target) {}
/** The heuristic function. @param pos the position to calculate the heuristic for
* @return an (over) estimate of the cost to target */
float operator()(const Vec2i &pos) const {
float dx = (float)abs(pos.x - target.x),
dy = (float)abs(pos.y - target.y);
float diag = dx < dy ? dx : dy;
float estimate = 1.4 * diag + (dx + dy - 2 * diag);
estimate *= 1.25;
return estimate;
}
};
/** Diagonal Distance underestimating Heuristic */
class UnderEstimate {
private:
Vec2i target; /**< search target */
public:
UnderEstimate(const Vec2i &target) : target(target) {}
/** The heuristic function. @param pos the position to calculate the heuristic for
* @return an (over) estimate of the cost to target */
float operator()(const Vec2i &pos) const {
float dx = (float)abs(pos.x - target.x),
dy = (float)abs(pos.y - target.y);
float diag = dx < dy ? dx : dy;
float estimate = 1.4 * diag + (dx + dy - 2 * diag);
estimate *= 0.75;
return estimate;
}
};
/** The Zero Heuristic, for doing Dijkstra searches */
class ZeroHeuristic {
public:
ZeroHeuristic(){}
/** The 'no heuristic' function. @param pos the position to ignore @return 0.f */
float operator()(const Vec2i &pos) const { return 0.0f; }
};

View File

@ -478,6 +478,8 @@ void Game::init()
logger.add("Launching game");
SystemFlags::OutputDebug(SystemFlags::debugSystem,"================ STARTING GAME ================\n");
SystemFlags::OutputDebug(SystemFlags::debugPathFinder,"================ STARTING GAME ================\n");
SystemFlags::OutputDebug(SystemFlags::debugPathFinder,"PathFinderType: %s\n", (getGameSettings()->getPathFinderType() ? "RoutePlanner" : "PathFinder"));
SystemFlags::OutputDebug(SystemFlags::debugSystem,"In [%s::%s Line: %d]\n",__FILE__,__FUNCTION__,__LINE__);
}

View File

@ -239,6 +239,7 @@ void MenuStateScenario::loadGameSettings(const ScenarioInfo *scenarioInfo, GameS
gameSettings->setFactionCount(factionCount);
gameSettings->setFogOfWar(scenarioInfo->fogOfWar);
gameSettings->setPathFinderType(pfRoutePlanner);
}
ControlType MenuStateScenario::strToControllerType(const string &str){