- improved the new pathfinder using hashmap

This commit is contained in:
Mark Vejvoda 2012-05-05 06:23:09 +00:00
parent f79222defa
commit 1b2b1f9821
5 changed files with 1331 additions and 350 deletions

View File

@ -0,0 +1,400 @@
// ==============================================================
// This file is part of Glest (www.glest.org)
//
// Copyright (C) 2012 Mark Vejvoda
//
// 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 __INTRUSIVE_HASH_SET_HPP__
#define __INTRUSIVE_HASH_SET_HPP__
#include <inttypes.h>
#include <memory.h>
/*
This is base class for values that can be
stored in intrusive hashset.
*/
struct IntrHashSetNodeBase{
IntrHashSetNodeBase* ihsNextNode;
};
/*
Values must be allocated in (some kind of) a heap.
Not stack.
Values are also nodes in a hash table,
so deleting values that is in hash is bad thing to do.
*/
/*
class TRAITS MUST implement following static members:
bool TRAITS::isEqual(const T* a,const T* b);
uint32_t TRAITS::getHashCode(const T* a);
*/
template <class T,class TRAITS>
class IntrHashSet{
protected:
typedef T Node;
// copy constructor is protected
// values CANNOT be copied since
// they are allocated outside of this container!
IntrHashSet(const IntrHashSet&);
public:
// default constructor
// preAlloc values CANNOT be zero!
IntrHashSet(int preAlloc=128)
{
hash=new Node*[preAlloc];
memset(hash,0,sizeof(Node*)*preAlloc);
hashSize=preAlloc;
collisionsCount=0;
}
//destructor
~IntrHashSet()
{
if(hash)
{
delete [] hash;
}
}
//just empties container without deleting values!
void Clear()
{
collisionsCount=0;
memset(hash,0,sizeof(Node*)*hashSize);
}
//insert allocated value
void Insert(T* value)
{
if(collisionsCount>hashSize/2)
{
Rehash();
}
uint32_t hidx=getIndex(value);
Node*& nodePtr=hash[hidx];
collisionsCount+=nodePtr!=0?1:0;
value->ihsNextNode=nodePtr;
nodePtr=value;
}
//iterator
//can be used to iterate over all values of hash
//and also returned by find method and can be
//use to delete value from hash faster
class ConstIterator{
public:
ConstIterator(const IntrHashSet* argHash):node(0),parent(0),index(0),hash(argHash){}
ConstIterator(const IntrHashSet& argHash):node(0),parent(0),index(0),hash(&argHash){}
const T* Get()const
{
return node;
}
bool Found()const
{
return node!=0;
}
void Rewind()
{
index=0;
node=0;
parent=0;
}
bool Next()
{
if(index>=hash->hashSize)
{
return false;
}
if(index==0 && node==0 && parent==0)
{
parent=&hash->hash;
node=*parent;
if(node)
{
return true;
}
}
while(node==0)
{
index++;
if(index>=hash->hashSize)
{
return false;
}
parent=hash->hash+index;
node=*parent;
if(node)
{
return true;
}
}
if(*parent!=node)
{
parent=(Node**)&(*parent)->ihsNextNode;
}
node=(Node*)node->ihsNextNode;
if(node)
{
return true;
}
return Next();
}
protected:
ConstIterator(const IntrHashSet* argHash,uint32_t argIndex,const Node* argNode,const Node*const* argParent):
node(argNode),parent(argParent),index(argIndex),hash(argHash){}
const Node* node;
const Node*const* parent;
uint32_t index;
const IntrHashSet* hash;
friend class IntrHashSet;
};
//mutable version of iterator
//however it's not good idea to modify key part of stored value
class Iterator{
public:
Iterator(IntrHashSet* argHash):node(0),parent(0),index(0),hash(argHash){}
Iterator(IntrHashSet& argHash):node(0),parent(0),index(0),hash(&argHash){}
T* Get()
{
return node;
}
bool Found()const
{
return node!=0;
}
void Rewind()
{
index=0;
node=0;
parent=0;
}
bool Next()
{
if(index>=hash->hashSize)
{
return false;
}
if(index==0 && node==0 && parent==0)
{
parent=hash->hash;
node=*parent;
if(node)
{
return true;
}
}
while(node==0)
{
index++;
if(index>=hash->hashSize)
{
return false;
}
parent=hash->hash+index;
node=*parent;
if(node)
{
return true;
}
}
if(*parent!=node)
{
parent=(Node**)&(*parent)->ihsNextNode;
}
node=(Node*)node->ihsNextNode;
if(node)
{
return true;
}
return Next();
}
protected:
Iterator(IntrHashSet* argHash,uint32_t argIndex,Node* argNode,Node** argParent):
node(argNode),parent(argParent),index(argIndex),hash(argHash){}
Node* node;
Node** parent;
uint32_t index;
IntrHashSet* hash;
friend class IntrHashSet;
};
//constant version of find
//returned iterator can be used to obtain found value
//or delete it from hashet
ConstIterator Find(const T& value)const
{
uint32_t hidx=getIndex(&value);
Node*const* nodePtr=&hash[hidx];
if(!*nodePtr)return ConstIterator(0);
if(TRAITS::isEqual((*nodePtr),&value))
{
return ConstIterator(this,hidx,*nodePtr,nodePtr);
}
Node*const* parentPtr=nodePtr;
Node* node=(Node*)(*nodePtr)->ihsNextNode;
while(node)
{
if(TRAITS::isEqual(node,&value))
{
return ConstIterator(this,hidx,node,parentPtr);
}
parentPtr=nodePtr;
nodePtr=(Node**)&node->ihsNextNode;
node=(Node*)node->ihsNextNode;
}
return ConstIterator(0);
}
//non constant version of find
Iterator Find(const T& value)
{
uint32_t hidx=getIndex(&value);
Node** nodePtr=&hash[hidx];
if(!*nodePtr)return Iterator(0);
if(TRAITS::isEqual((*nodePtr),&value))
{
return Iterator(this,hidx,*nodePtr,nodePtr);
}
Node** parentPtr=nodePtr;
Node* node=(Node*)(*nodePtr)->ihsNextNode;
while(node)
{
if(TRAITS::isEqual(node,&value))
{
return Iterator(this,hidx,node,parentPtr);
}
parentPtr=nodePtr;
nodePtr=(Node**)&node->ihsNextNode;
node=(Node*)node->ihsNextNode;
}
return Iterator(0);
}
//delete element from hashset 'by value'
//actual value IS NOT DEALLOCATED
//and should be deallocated separately
void Delete(const T& value)
{
uint32_t hidx=getIndex(&value);
Node** nodePtr=hash+hidx;
if(TRAITS::isEqual((*nodePtr),&value))
{
Node* node=*nodePtr;
*nodePtr=(Node*)node->ihsNextNode;
if(*nodePtr)
{
collisionsCount--;
}
return;
}
Node* parentPtr=*nodePtr;
Node* node=(Node*)(*nodePtr)->ihsNextNode;
while(node)
{
if(TRAITS::isEqual(node,&value))
{
parentPtr->ihsNextNode=node->ihsNextNode;
return;
}
parentPtr=node;
node=(Node*)node->ihsNextNode;
}
}
//delete element by iterator returned from Find
//or used for iteration.
//iterator used for Delete is no longer valid
//for iteration, however element it refer to
//is still here and Get is still valid after Delete.
//actuall value also IS NOT DEALLOCATED!
void Delete(const Iterator& hi)
{
Node** parentPtr=(Node**)hi.parent;
Node* node=(Node*)hi.node;
if(!node)
{
return;
}
if(*parentPtr==node)
{
*parentPtr=(Node*)node->ihsNextNode;
}else
{
(*parentPtr)->ihsNextNode=node->ihsNextNode;
}
if(*parentPtr)
{
collisionsCount--;
}
}
protected:
// get index of a bucket in a hash table
uint32_t getIndex(const T* value)const
{
uint32_t hashCode=TRAITS::getHashCode(value);
return hashCode%hashSize;
}
// increase hash table size if there are
// too much collisions
void Rehash()
{
Node** oldHash=hash;
Node** oldHashEnd=hash+hashSize;
hashSize*=2;
hash=new Node*[hashSize];
memset(hash,0,sizeof(Node*)*hashSize);
collisionsCount=0;
for(Node** it=oldHash;it!=oldHashEnd;it++)
{
Node* node=*it;
Node* nextNode;
while(node)
{
uint32_t hidx=getIndex(node);
Node** nodePtr=hash+hidx;
if(*nodePtr)
{
collisionsCount++;
}
nextNode=(Node*)node->ihsNextNode;
node->ihsNextNode=*nodePtr;
*nodePtr=node;
node=nextNode;
}
}
}
// hash table itself
Node** hash;
// size of hash table
size_t hashSize;
// number of collisions
int collisionsCount;
};
#endif

View File

@ -0,0 +1,543 @@
// ==============================================================
// This file is part of Glest (www.glest.org)
//
// Copyright (C) 2012 Mark Vejvoda
//
// 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 __INTRUSIVE_HEAP_HASH_SET_HPP__
#define __INTRUSIVE_HEAP_HASH_SET_HPP__
#include <inttypes.h>
#include <vector>
#include "IntrHashSet.hpp"
/*
Here is temlate class Intrusive Heap/HashSet.
Actuall values are stored in this container,
no copying is performed, so all values should
be allocated in a heap or in a storage with
lifetime as long as lifetime of container.
Values are also used as nodes of container,
so no value can be stored in two containers
of this kind simultaneously.
*/
/*
This is what should be done with duplicate
from hash point of view values.
*/
enum HHDuplicatePolicy{
hhdpReplaceOld, //replace old value. old values is not deallocated!
hhdpKeepOld, //container is not modified in this case.
hhdpInsertDuplicate //duplicates are stored in container
};
/*
TRAITS class MUST implement following static members:
for heap - this is something like operator< or operator>
bool TRAITS::Compare(const T* a,const T* b);
for hash
bool TRAITS::isEqual(const T* a,const T* b);
uint32_t TRAITS::getHashCode(const T& a);
*/
/*
This is base class for values stored in a intrusive heap hash
*/
struct IntrHeapHashNodeBase:public IntrHashSetNodeBase{
int ihhNodeIndex;
};
template <class T,class TRAITS,HHDuplicatePolicy policy=hhdpReplaceOld>
class IntrHeapHash{
public:
//default constructor
//preAlloc value cannot be 0!
IntrHeapHash(int preAlloc=128)
{
hash=new Node*[preAlloc];
memset(hash,0,preAlloc*sizeof(Node*));
hashSize=preAlloc;
heap=new Node*[preAlloc];
memset(heap,0,preAlloc*sizeof(Node*));
heapSize=0;
heapAlloc=preAlloc;
collisionsCount=0;
}
//guess what? destructor!
~IntrHeapHash()
{
delete [] hash;
delete [] heap;
}
// clear content container
// values are not deallocated!
void Clear()
{
memset(hash,0,hashSize*sizeof(Node*));
heapSize=0;
collisionsCount=0;
}
//insert new value into container
//return value is equal to inserted value for all policies if no duplicate detected
//in case of duplicate for keep old policy 0 is returned
//in case of duplicate for replace old policy old value is returned
T* Insert(T* value)
{
if(collisionsCount>hashSize/2)
{
Rehash();
}
uint32_t hidx=getIndex(value);
Node*& node=hash[hidx];
T* rv=value;
if(policy==hhdpReplaceOld || policy==hhdpKeepOld)
{
Node* ptr=FindHashNode(node,*value);
if(ptr)
{
if(policy==hhdpKeepOld)
{
return 0;
}
DeleteHashNode(hash+hidx,ptr);
rv=ptr;
}
}
value->ihhNodeIndex=heapSize;
if(node)
{
collisionsCount++;
}
value->ihsNextNode=node;
node=value;
if(heapSize==heapAlloc)
{
heapAlloc*=2;
Node** newHeap=new Node*[heapAlloc];
memcpy(newHeap,heap,heapSize*sizeof(Node*));
delete [] heap;
heap=newHeap;
}
heap[heapSize++]=value;
HeapPush();
return rv;
}
// value at the head of heap
const T* getHead()const
{
return *heap;
}
// same as previous but non const
T* getHead()
{
return **heap;
}
// get and remove head value of heap
T* Pop()
{
T* rv=*heap;
uint32_t hidx=getIndex(rv);
DeleteHashNode(&hash[hidx],rv);
HeapSwap(heap,heap+heapSize-1);
heapSize--;
HeapPop();
return rv;
}
//find value in hash
//const version
const T* Find(const T& value)const
{
uint32_t hidx=getIndex(&value);
Node* ptr=hash[hidx];
while(ptr)
{
if(TRAITS::isEqual(ptr,&value))
{
return ptr;
}
ptr=(Node*)ptr->ihsNextNode;
}
return 0;
}
//find value in hash
//non const version
//it's not good idea to modify key value of hash or key value of heap however
T* Find(const T& value)
{
uint32_t hidx=getIndex(&value);
Node* ptr=hash[hidx];
while(ptr)
{
if(TRAITS::isEqual(ptr,&value))
{
return ptr;
}
ptr=(Node*)ptr->ihsNextNode;
}
return 0;
}
//get from hash all values equal to given
void GetAll(const T& value,std::vector<T*>& values)const
{
uint32_t hidx=getIndex(&value);
Node* ptr=hash[hidx];
while(ptr)
{
if(TRAITS::isEqual(ptr,&value))
{
values.push_back(ptr);
}
ptr=(Node*)ptr->ihsNextNode;
}
}
//delete value from hash
//deleted object returned
T* Delete(const T& value)
{
uint32_t hidx=getIndex(&value);
int idx=DeleteHashNode(hash+hidx,value);
if(idx==-1)
{
return 0;
}
if(idx==heapSize-1)
{
heapSize--;
return heap[heapSize];
}
HeapSwap(heap+idx,heap+heapSize-1);
heapSize--;
HeapFix(idx);
return heap[heapSize];
}
//delete all values equal to given
void DeleteAll(const T& value)
{
uint32_t hidx=getIndex(&value);
Node** nodePtr=hash+hidx;
while(*nodePtr)
{
int idx=DeleteHashNode(nodePtr,value);
if(idx==-1)
{
return;
}
if(idx==heapSize-1)
{
heapSize--;
continue;
}
HeapSwap(heap+idx,heap+heapSize-1);
heapSize--;
HeapFix(idx);
}
}
//get number of elements in container
size_t getSize()const
{
return heapSize;
}
//check if container is empty
bool isEmpty()const
{
return heapSize==0;
}
//iterator
//can/should be used like this:
//for(IHHTypeDef::Iterator it(ihh);it.Next();)
//{
// dosomething(it.Get())
//}
class Iterator{
public:
Iterator(const IntrHeapHash& argHH)
{
begin=argHH.heap;
current=0;
end=argHH.heap+argHH.heapSize;
}
bool Next()
{
if(current==0)
{
current=begin;
}else
{
current++;
}
return current!=end;
}
T* Get()
{
if(current)
{
return *current;
}
return 0;
}
void Rewind()
{
current=begin;
}
protected:
typename IntrHeapHash::Node **begin,**current,**end;
};
protected:
//protected copy constructor
//values cannot be copied as well as container
IntrHeapHash(const IntrHeapHash&);
//typedef for convenience
typedef T Node;
//heap array
Node** heap;
//hash table
Node** hash;
//size of hash table
size_t hashSize;
//used size of heap
size_t heapSize;
//allocated size of heap
size_t heapAlloc;
//count of collisions in hash
int collisionsCount;
//get index of value in hash table
uint32_t getIndex(const T* value)const
{
uint32_t hashCode=TRAITS::getHashCode(value);
return hashCode%hashSize;
}
//resize hash table when
//collisions count is too big
void Rehash()
{
Node** oldHash=hash;
Node** oldHashEnd=oldHash+hashSize;
hashSize*=2;
hash=new Node*[hashSize];
memset(hash,0,hashSize*sizeof(Node*));
collisionsCount=0;
for(Node** it=oldHash;it!=oldHashEnd;it++)
{
Node* nodePtr=*it;
while(nodePtr)
{
uint32_t hidx=getIndex(nodePtr);
Node*& hnode=hash[hidx];
if(hnode)
{
collisionsCount++;
}
Node* next=(Node*)nodePtr->ihsNextNode;
nodePtr->ihsNextNode=hnode;
hnode=nodePtr;
nodePtr=next;
}
}
}
//find and delete hash node from bucket by ptr
void DeleteHashNode(Node** nodeBasePtr,Node* nodePtr)
{
if(*nodeBasePtr==nodePtr)
{
*nodeBasePtr=(Node*)nodePtr->ihsNextNode;
if(*nodeBasePtr)
{
collisionsCount--;
}
return;
}
Node* parentPtr=*nodeBasePtr;
Node* iterPtr=(Node*)parentPtr->ihsNextNode;
while(iterPtr)
{
if(iterPtr==nodePtr)
{
parentPtr->ihsNextNode=nodePtr->ihsNextNode;
collisionsCount--;
return;
}
parentPtr=iterPtr;
iterPtr=(Node*)iterPtr->ihsNextNode;
}
}
//find and delete hash node from bucked by value
//heap index returned
int DeleteHashNode(Node** nodePtr,const T& data)
{
if(!*nodePtr)return -1;
if(TRAITS::isEqual(*nodePtr,&data))
{
int rv=(*nodePtr)->ihhNodeIndex;
*nodePtr=(Node*)(*nodePtr)->ihsNextNode;
if(*nodePtr)
{
collisionsCount--;
}
return rv;
}
Node* parentPtr=*nodePtr;
Node* node=(Node*)(*nodePtr)->ihsNextNode;
while(node)
{
if(TRAITS::isEqual(node,&data))
{
parentPtr->ihsNextNode=node->ihsNextNode;
collisionsCount--;
return node->ihhNodeIndex;
}
parentPtr=node;
node=(Node*)node->ihsNextNode;
}
return -1;
}
//find hash node in a bucked by value
Node* FindHashNode(Node* nodePtr,const T& data)
{
while(nodePtr)
{
if(TRAITS::isEqual(nodePtr,&data))
{
return nodePtr;
}
nodePtr=(Node*)nodePtr->ihsNextNode;
}
return 0;
}
//float up tail element
void HeapPush(int didx=-1)
{
int idx=didx==-1?heapSize-1:didx;
int pidx;
Node** b=heap;
Node** i=b+idx;
Node** pi;
Node* ptr=*i;
while(idx)
{
pidx=(idx-1)/2;
pi=b+pidx;
if(TRAITS::Compare(ptr,(*pi)))
{
*i=*pi;
(*i)->ihhNodeIndex=idx;
i=pi;
idx=pidx;
}else
{
break;
}
}
*i=ptr;
ptr->ihhNodeIndex=idx;
}
//drow down element from head
void HeapPop(int idx=0)
{
if(heapSize<=1)
{
return;
}
Node **i,**b,**i1,**i2;
int cidx1,cidx2;
b=heap;
i=b+idx;
Node* ptr=*i;
while(idx<heapSize)
{
cidx1=idx*2+1;
cidx2=idx*2+2;
if(cidx1>=heapSize)
{
break;
}
i1=b+cidx1;
i2=b+cidx2;
if(cidx2>=heapSize || TRAITS::Compare((*i1),(*i2)))
{
if(!TRAITS::Compare(ptr,(*i1)))
{
*i=*i1;
(*i)->ihhNodeIndex=idx;
idx=cidx1;
i=i1;
}else
{
break;
}
}else
{
if(!TRAITS::Compare(ptr,(*i2)))
{
*i=*i2;
(*i)->ihhNodeIndex=idx;
idx=cidx2;
i=i2;
}else
{
break;
}
}
}
*i=ptr;
ptr->ihhNodeIndex=idx;
}
//either drow down or float up element at middle
void HeapFix(int idx)
{
int pidx=(idx-1)/2;
if(TRAITS::Compare(heap[idx],heap[pidx]))
{
HeapPush(idx);
}else
{
HeapPop(idx);
}
}
//swap two element fixing indeces
void HeapSwap(Node** i1,Node** i2)
{
int idx1=(*i1)->ihhNodeIndex;
int idx2=(*i2)->ihhNodeIndex;
Node* ptr=*i1;
*i1=*i2;
(*i1)->ihhNodeIndex=idx1;
(*i2)=ptr;
(*i2)->ihhNodeIndex=idx2;
}
};
#endif

View File

@ -118,6 +118,7 @@ Copyright (C)2001-2005 Justin Heyes-Jones
//*******************************
#include "fast_path_finder.h"
#include "IntrHeapHash.hpp"
template <class USER_TYPE> class FixedSizeAllocator {
@ -358,7 +359,7 @@ public: // data
public:
class Node {
class Node : public IntrHeapHashNodeBase {
public:
Node *parent; // used during the search to record the parent of successor nodes
Node *child; // used after the search for the application to view the search in reverse
@ -376,30 +377,67 @@ public:
{
}
//used in AllocateNode
void clear() {
parent=0;
child=0;
g=0;
h=0;
f=0;
}
UserState m_UserState;
};
// Both hashset and heaphash use this traits of node
struct NodeTraits{
static bool Compare(const Node* x,const Node* y)
{
return x->f < y->f;
}
static bool isEqual(const Node* x,const Node* y)
{
return x->m_UserState.IsSameState(y->m_UserState);
}
static int32 getHashCode(const Node* x)
{
return x->m_UserState.getHashCode();
}
};
// For sorting the heap the STL needs compare function that lets us compare
// the f value of two nodes
class HeapCompare_f {
public:
bool operator() ( const Node *x, const Node *y ) const {
return x->f > y->f;
}
};
// class HeapCompare_f {
// public:
// bool operator() ( const Node *x, const Node *y ) const {
// return x->f > y->f;
// }
// };
public: // methods
// constructor just initialises private data
AStarSearch( int MaxNodes = 2000 ) :
m_AllocateNodeCount(0),
m_FixedSizeAllocator( MaxNodes ),
AStarSearch( ) :
m_State( SEARCH_STATE_NOT_INITIALISED ),
m_CurrentSolutionNode( NULL ),
m_Start( NULL),
m_Goal( NULL),
m_CancelRequest( false )
{
InitPool();
}
~AStarSearch()
{
// if additional pool page was allocated, let's free it
NodePoolPage* page=defaultPool.nextPage;
while(page)
{
NodePoolPage* next=page->nextPage;
delete page;
page=next;
}
}
// call at any time to cancel the search and free up all the memory
@ -411,28 +449,23 @@ public: // methods
void SetStartAndGoalStates( UserState &Start, UserState &Goal, void *userData ) {
this->userData = userData;
m_FixedSizeAllocator.reset();
m_AllocateNodeCount = 0;
m_OpenList.clear();
m_ClosedList.clear();
m_Successors.clear();
//m_OpenList.reserve(m_FixedSizeAllocator);
//m_ClosedList.reserve(m_FixedSizeAllocator);
//m_Successors.reserve(m_FixedSizeAllocator);
// Reinit nodes pool after previous searches
InitPool();
m_CurrentSolutionNode = NULL;
// Clear containers after previous searches
m_OpenList.Clear();
m_ClosedList.Clear();
m_CancelRequest = false;
m_CancelRequest = false;
m_Start = AllocateNode();
m_Goal = AllocateNode();
// allocate start end goal nodes
m_Start = AllocateNode();
m_Goal = AllocateNode();
assert((m_Start != NULL && m_Goal != NULL));
m_Start->m_UserState = Start;
m_Goal->m_UserState = Goal;
m_Start->m_UserState = Start;
m_Goal->m_UserState = Goal;
m_State = SEARCH_STATE_SEARCHING;
m_State = SEARCH_STATE_SEARCHING;
// Initialise the AStar specific parts of the Start Node
// The user only needs fill out the state information
@ -442,10 +475,11 @@ public: // methods
m_Start->parent = 0;
// Push the start node on the Open list
m_OpenList.push_back( m_Start ); // heap now unsorted
//m_OpenList.push_back( m_Start ); // heap now unsorted
// Sort back element into heap
push_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
//push_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
m_OpenList.Insert( m_Start );
// Initialise counter for search steps
m_Steps = 0;
@ -465,8 +499,8 @@ public: // methods
// Failure is defined as emptying the open list as there is nothing left to
// search...
// New: Allow user abort
if( m_OpenList.empty() || m_CancelRequest ) {
FreeAllNodes();
if( m_OpenList.isEmpty() || m_CancelRequest ) {
//FreeAllNodes();
m_State = SEARCH_STATE_FAILED;
return m_State;
}
@ -474,176 +508,137 @@ public: // methods
// Incremement step count
m_Steps ++;
// Pop the best node (the one with the lowest f)
Node *n = m_OpenList.front(); // get pointer to the node
pop_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
m_OpenList.pop_back();
// Pop the best node (the one with the lowest f)
Node *n= m_OpenList.Pop();
// Check for the goal, once we pop that we're done
if( n->m_UserState.IsGoal( m_Goal->m_UserState ) ) {
// The user is going to use the Goal Node he passed in
// so copy the parent pointer of n
m_Goal->parent = n->parent;
//printf("n:(%d,%d):%d\n",n->m_UserState.x,n->m_UserState.y,n->f);
// A special case is that the goal was passed in as the start state
// so handle that here
if( false == n->m_UserState.IsSameState( m_Start->m_UserState ) ) {
FreeNode( n );
// Check for the goal, once we pop that we're done
if( n->m_UserState.IsGoal( m_Goal->m_UserState ) )
{
// The user is going to use the Goal Node he passed in
// so copy the parent pointer of n
m_Goal->parent = n->parent;
// set the child pointers in each node (except Goal which has no child)
Node *nodeChild = m_Goal;
Node *nodeParent = m_Goal->parent;
// A special case is that the goal was passed in as the start state
// so handle that here
if( false == n->m_UserState.IsSameState( m_Start->m_UserState ) )
{
FreeNode( n );
do {
nodeParent->child = nodeChild;
// set the child pointers in each node (except Goal which has no child)
Node *nodeChild = m_Goal;
Node *nodeParent = m_Goal->parent;
do
{
nodeParent->child = nodeChild;
nodeChild = nodeParent;
nodeParent = nodeParent->parent;
nodeChild = nodeParent;
nodeParent = nodeParent->parent;
}
while( nodeChild != m_Start ); // Start is always the first node by definition
}
}
while( nodeChild != m_Start ); // Start is always the first node by definition
// delete nodes that aren't needed for the solution
FreeUnusedNodes();
}
m_State = SEARCH_STATE_SUCCEEDED;
m_State = SEARCH_STATE_SUCCEEDED;
return m_State;
}
// not goal
else {
// We now need to generate the successors of this node
// The user helps us to do this, and we keep the new nodes in
// m_Successors ...
m_Successors.clear(); // empty vector of successor nodes to n
return m_State;
}
else // not goal
{
// User provides this functions and uses AddSuccessor to add each successor of
// node 'n' to m_Successors
bool ret = n->m_UserState.GetSuccessors( this, n->parent ? &n->parent->m_UserState : NULL );
// We now need to generate the successors of this node
// The user helps us to do this, and we keep the new nodes in
// m_Successors ...
if( !ret ) {
typename vector< Node * >::iterator successor;
m_Successors.clear(); // empty vector of successor nodes to n
// free the nodes that may previously have been added
for( successor = m_Successors.begin(); successor != m_Successors.end(); successor ++ ) {
FreeNode( (*successor) );
}
// User provides this functions and uses AddSuccessor to add each successor of
// node 'n' to m_Successors
n->m_UserState.GetSuccessors( this, n->parent ? &n->parent->m_UserState : NULL );
m_Successors.clear(); // empty vector of successor nodes to n
// Now handle each successor to the current node ...
for( typename std::vector< Node * >::iterator successor = m_Successors.begin(); successor != m_Successors.end(); successor ++ )
{
// free up everything else we allocated
FreeAllNodes();
// The g value for this successor ...
int newg = n->g + n->m_UserState.GetCost( (*successor)->m_UserState, userData );
m_State = SEARCH_STATE_OUT_OF_MEMORY;
return m_State;
}
// Now handle each successor to the current node ...
for( typename vector< Node * >::iterator successor = m_Successors.begin();
successor != m_Successors.end(); successor ++ ) {
searchCount++;
// Now we need to find whether the node is on the open or closed lists
// If it is but the node that is already on them is better (lower g)
// then we can forget about this successor
// The g value for this successor ...
float newg = n->g + n->m_UserState.GetCost( (*successor)->m_UserState, userData );
const Node* openlist_result=m_OpenList.Find(**successor);
// Now we need to find whether the node is on the open or closed lists
// If it is but the node that is already on them is better (lower g)
// then we can forget about this successor
if( openlist_result )
{
// First linear search of open list to find node
// we found this state on open
typename vector< Node * >::iterator openlist_result;
if( openlist_result->g <= newg )
{
FreeNode( (*successor) );
for( openlist_result = m_OpenList.begin();
openlist_result != m_OpenList.end(); openlist_result ++ ) {
if( (*openlist_result)->m_UserState.IsSameState( (*successor)->m_UserState ) ) {
break;
}
}
// the one on Open is cheaper than this one
continue;
}
}
if( openlist_result != m_OpenList.end() ) {
// we found this state on open
if( (*openlist_result)->g <= newg ) {
FreeNode( (*successor) );
typename NodeHash::Iterator closedlist_result=m_ClosedList.Find(**successor);
// the one on Open is cheaper than this one
continue;
}
}
if( closedlist_result.Found() )
{
typename vector< Node * >::iterator closedlist_result;
// we found this state on closed
for( closedlist_result = m_ClosedList.begin();
closedlist_result != m_ClosedList.end(); closedlist_result ++ ) {
if( (*closedlist_result)->m_UserState.IsSameState( (*successor)->m_UserState ) ) {
break;
}
}
if( closedlist_result.Get()->g <= newg )
{
// the one on Closed is cheaper than this one
FreeNode( (*successor) );
if( closedlist_result != m_ClosedList.end() ) {
// we found this state on closed
if( (*closedlist_result)->g <= newg ) {
// the one on Closed is cheaper than this one
FreeNode( (*successor) );
continue;
}
}
continue;
}
}
// This node is the best node so far with this particular state
// so lets keep it and set up its AStar specific data ...
// This node is the best node so far with this particular state
// so lets keep it and set up its AStar specific data ...
(*successor)->parent = n;
(*successor)->g = newg;
(*successor)->h = (*successor)->m_UserState.GoalDistanceEstimate( m_Goal->m_UserState, userData );
(*successor)->f = (*successor)->g + (*successor)->h;
(*successor)->parent = n;
(*successor)->g = newg;
(*successor)->h = (*successor)->m_UserState.GoalDistanceEstimate( m_Goal->m_UserState, userData );
(*successor)->f = (*successor)->g + (*successor)->h;
// Remove successor from closed if it was on it
// Remove successor from closed if it was on it
if( closedlist_result != m_ClosedList.end() ) {
// remove it from Closed
FreeNode( (*closedlist_result) );
m_ClosedList.erase( closedlist_result );
if( closedlist_result.Found() )
{
// remove it from Closed
Node* node=(Node*)closedlist_result.Get();
m_ClosedList.Delete( closedlist_result );
FreeNode( node );
// Fix thanks to ...
// Greg Douglas <gregdouglasmail@gmail.com>
// who noticed that this code path was incorrect
// Here we have found a new state which is already CLOSED
// anus
}
}
// Update old version of this node
if( openlist_result != m_OpenList.end() ) {
FreeNode( (*openlist_result) );
m_OpenList.erase( openlist_result );
// Update old version of this node
if( openlist_result )
{
m_OpenList.Delete( *openlist_result );
FreeNode( (Node*)openlist_result );
}
// re-make the heap
// make_heap rather than sort_heap is an essential bug fix
// thanks to Mike Ryynanen for pointing this out and then explaining
// it in detail. sort_heap called on an invalid heap does not work
make_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
}
m_OpenList.Insert( (*successor) );
// heap now unsorted
m_OpenList.push_back( (*successor) );
}
// if (m_OpenList[0]->f == m_OpenList[m_OpenList.size()-1]->f) {
// Node *aux = m_OpenList[0];
// m_OpenList[0] = m_OpenList[m_OpenList.size()-1];
// m_OpenList[m_OpenList.size()-1] = aux;
// }
// sort back element into heap
push_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() );
// push n onto Closed, as we have expanded it now
}
m_ClosedList.Insert( n );
// push n onto Closed, as we have expanded it now
} // end else (not goal so expand)
m_ClosedList.push_back( n );
} // end else (not goal so expand)
return m_State; // Succeeded bool is false at this point.
return m_State; // Succeeded bool is false at this point.
}
// User calls this to add a successor to a list of successors
@ -707,9 +702,8 @@ public: // methods
UserState *GetSolutionNext() {
if( m_CurrentSolutionNode ) {
if( m_CurrentSolutionNode->child ) {
Node *child = m_CurrentSolutionNode->child;
m_CurrentSolutionNode = m_CurrentSolutionNode->child;
return &child->m_UserState;
m_CurrentSolutionNode = m_CurrentSolutionNode->child;
return &m_CurrentSolutionNode->m_UserState;
}
}
@ -731,9 +725,8 @@ public: // methods
UserState *GetSolutionPrev() {
if( m_CurrentSolutionNode ) {
if( m_CurrentSolutionNode->parent ) {
Node *parent = m_CurrentSolutionNode->parent;
m_CurrentSolutionNode = m_CurrentSolutionNode->parent;
return &parent->m_UserState;
m_CurrentSolutionNode = m_CurrentSolutionNode->parent;
return &m_CurrentSolutionNode->m_UserState;
}
}
@ -742,192 +735,230 @@ public: // methods
// For educational use and debugging it is useful to be able to view
// the open and closed list at each step, here are two functions to allow that.
UserState *GetOpenListStart() {
float f,g,h;
return GetOpenListStart( f,g,h );
}
// UserState *GetOpenListStart() {
// float f,g,h;
// return GetOpenListStart( f,g,h );
// }
UserState *GetOpenListStart( float &f, float &g, float &h ) {
iterDbgOpen = m_OpenList.begin();
if( iterDbgOpen != m_OpenList.end() ) {
f = (*iterDbgOpen)->f;
g = (*iterDbgOpen)->g;
h = (*iterDbgOpen)->h;
return &(*iterDbgOpen)->m_UserState;
}
// UserState *GetOpenListStart( float &f, float &g, float &h ) {
// iterDbgOpen = m_OpenList.begin();
// if( iterDbgOpen != m_OpenList.end() ) {
// f = (*iterDbgOpen)->f;
// g = (*iterDbgOpen)->g;
// h = (*iterDbgOpen)->h;
// return &(*iterDbgOpen)->m_UserState;
// }
//
// return NULL;
// }
return NULL;
}
// UserState *GetOpenListNext() {
// float f,g,h;
// return GetOpenListNext( f,g,h );
// }
UserState *GetOpenListNext() {
float f,g,h;
return GetOpenListNext( f,g,h );
}
// UserState *GetOpenListNext( float &f, float &g, float &h ) {
// iterDbgOpen++;
// if( iterDbgOpen != m_OpenList.end() ) {
// f = (*iterDbgOpen)->f;
// g = (*iterDbgOpen)->g;
// h = (*iterDbgOpen)->h;
// return &(*iterDbgOpen)->m_UserState;
// }
//
// return NULL;
// }
UserState *GetOpenListNext( float &f, float &g, float &h ) {
iterDbgOpen++;
if( iterDbgOpen != m_OpenList.end() ) {
f = (*iterDbgOpen)->f;
g = (*iterDbgOpen)->g;
h = (*iterDbgOpen)->h;
return &(*iterDbgOpen)->m_UserState;
}
return NULL;
}
UserState *GetClosedListStart() {
float f,g,h;
return GetClosedListStart( f,g,h );
}
UserState *GetClosedListStart( float &f, float &g, float &h ) {
iterDbgClosed = m_ClosedList.begin();
if( iterDbgClosed != m_ClosedList.end() ) {
f = (*iterDbgClosed)->f;
g = (*iterDbgClosed)->g;
h = (*iterDbgClosed)->h;
return &(*iterDbgClosed)->m_UserState;
}
return NULL;
}
UserState *GetClosedListNext() {
float f,g,h;
return GetClosedListNext( f,g,h );
}
UserState *GetClosedListNext( float &f, float &g, float &h ) {
iterDbgClosed++;
if( iterDbgClosed != m_ClosedList.end() ) {
f = (*iterDbgClosed)->f;
g = (*iterDbgClosed)->g;
h = (*iterDbgClosed)->h;
return &(*iterDbgClosed)->m_UserState;
}
return NULL;
}
// UserState *GetClosedListStart() {
// float f,g,h;
// return GetClosedListStart( f,g,h );
// }
//
// UserState *GetClosedListStart( float &f, float &g, float &h ) {
// iterDbgClosed = m_ClosedList.begin();
// if( iterDbgClosed != m_ClosedList.end() ) {
// f = (*iterDbgClosed)->f;
// g = (*iterDbgClosed)->g;
// h = (*iterDbgClosed)->h;
//
// return &(*iterDbgClosed)->m_UserState;
// }
//
// return NULL;
// }
//
// UserState *GetClosedListNext() {
// float f,g,h;
// return GetClosedListNext( f,g,h );
// }
//
// UserState *GetClosedListNext( float &f, float &g, float &h ) {
// iterDbgClosed++;
// if( iterDbgClosed != m_ClosedList.end() ) {
// f = (*iterDbgClosed)->f;
// g = (*iterDbgClosed)->g;
// h = (*iterDbgClosed)->h;
//
// return &(*iterDbgClosed)->m_UserState;
// }
//
// return NULL;
// }
// Get the number of steps
int GetStepCount() { return m_Steps; }
inline int GetStepCount() { return m_Steps; }
void EnsureMemoryFreed() {
#if USE_FSA_MEMORY
assert(m_AllocateNodeCount == 0);
#endif
}
// void EnsureMemoryFreed() {
//#if USE_FSA_MEMORY
// assert(m_AllocateNodeCount == 0);
//#endif
// }
void * getUserData() { return userData; }
inline void * getUserData() { return userData; }
// This is called when a search fails or is cancelled to free all used
// memory
void FreeAllNodes() {
// iterate open list and delete all nodes
typename vector< Node * >::iterator iterOpen = m_OpenList.begin();
while( iterOpen != m_OpenList.end() ) {
Node *n = (*iterOpen);
FreeNode( n );
iterOpen ++;
}
m_OpenList.clear();
// iterate closed list and delete unused nodes
typename vector< Node * >::iterator iterClosed;
for( iterClosed = m_ClosedList.begin();
iterClosed != m_ClosedList.end(); iterClosed ++ ) {
Node *n = (*iterClosed);
FreeNode( n );
}
m_ClosedList.clear();
// delete the goal
FreeNode(m_Goal);
m_Goal = NULL;
}
// void FreeAllNodes() {
// // iterate open list and delete all nodes
// typename vector< Node * >::iterator iterOpen = m_OpenList.begin();
//
// while( iterOpen != m_OpenList.end() ) {
// Node *n = (*iterOpen);
// FreeNode( n );
//
// iterOpen ++;
// }
//
// m_OpenList.clear();
//
// // iterate closed list and delete unused nodes
// typename vector< Node * >::iterator iterClosed;
//
// for( iterClosed = m_ClosedList.begin();
// iterClosed != m_ClosedList.end(); iterClosed ++ ) {
// Node *n = (*iterClosed);
// FreeNode( n );
// }
//
// m_ClosedList.clear();
//
// // delete the goal
// FreeNode(m_Goal);
// m_Goal = NULL;
// }
private: // methods
// This call is made by the search class when the search ends. A lot of nodes may be
// created that are still present when the search ends. They will be deleted by this
// routine once the search ends
void FreeUnusedNodes() {
// iterate open list and delete unused nodes
typename vector< Node * >::iterator iterOpen = m_OpenList.begin();
while( iterOpen != m_OpenList.end() ) {
Node *n = (*iterOpen);
if( !n->child ) {
FreeNode( n );
n = NULL;
}
iterOpen ++;
}
m_OpenList.clear();
// iterate closed list and delete unused nodes
typename vector< Node * >::iterator iterClosed;
for( iterClosed = m_ClosedList.begin();
iterClosed != m_ClosedList.end(); iterClosed ++ ) {
Node *n = (*iterClosed);
if( !n->child ) {
FreeNode( n );
n = NULL;
}
}
m_ClosedList.clear();
}
// void FreeUnusedNodes() {
// // iterate open list and delete unused nodes
// typename vector< Node * >::iterator iterOpen = m_OpenList.begin();
//
// while( iterOpen != m_OpenList.end() ) {
// Node *n = (*iterOpen);
// if( !n->child ) {
// FreeNode( n );
// n = NULL;
// }
//
// iterOpen ++;
// }
//
// m_OpenList.clear();
//
// // iterate closed list and delete unused nodes
// typename vector< Node * >::iterator iterClosed;
//
// for( iterClosed = m_ClosedList.begin();
// iterClosed != m_ClosedList.end(); iterClosed ++ ) {
// Node *n = (*iterClosed);
// if( !n->child ) {
// FreeNode( n );
// n = NULL;
// }
// }
//
// m_ClosedList.clear();
// }
// Node memory management
Node *AllocateNode() {
#if !USE_FSA_MEMORY
Node *p = new Node;
return p;
#else
Node *address = m_FixedSizeAllocator.alloc();
if( !address ) {
return NULL;
}
m_AllocateNodeCount ++;
Node *p = new (address) Node;
return p;
#endif
}
// Node memory management
inline Node *AllocateNode() {
if(freeNodesList) {
Node* rv=freeNodesList;
freeNodesList=freeNodesList->child;
rv->clear();
return rv;
}
if(currentPoolPage->nextFreeNode==currentPoolPage->endFreeNode) {
currentPoolPage=currentPoolPage->nextPage=new NodePoolPage;
}
Node* rv=currentPoolPage->nextFreeNode++;
rv->clear();
return rv;
}
void FreeNode( Node *node ) {
m_AllocateNodeCount --;
#if !USE_FSA_MEMORY
delete node;
#else
m_FixedSizeAllocator.free( node );
#endif
}
inline void FreeNode( Node *node ) {
node->child=freeNodesList;
freeNodesList=node;
}
private: // data
// Heap (simple vector but used as a heap, cf. Steve Rabin's game gems article)
vector< Node *> m_OpenList;
// binary heap and hashset '2 in 1' container with 'open' nodes
typedef IntrHeapHash<Node,NodeTraits> NodeHeap;
NodeHeap m_OpenList;
// Closed list is a vector.
vector< Node * > m_ClosedList;
// hashset for 'closed' nodes
typedef IntrHashSet<Node,NodeTraits> NodeHash;
NodeHash m_ClosedList;
// Successors is a vector filled out by the user each type successors to a node
// are generated
vector< Node * > m_Successors;
// Successors is a vector filled out by the user each type successors to a node
// are generated
typedef std::vector< Node * > NodeVector;
NodeVector m_Successors;
//default page size
enum { nodesPerPage=4096 };
// page of pool of nodes for fast allocation
struct NodePoolPage {
Node nodes[nodesPerPage];
Node* nextFreeNode;
Node* endFreeNode;
NodePoolPage* nextPage;
NodePoolPage() {
Init();
nextPage=0;
}
void Init() {
nextFreeNode=nodes;
endFreeNode=nodes+nodesPerPage;
}
};
//first page of pool allocated along with AStarSearch object on stack
//or in data segement if static.
//should be enough for most tasks
NodePoolPage defaultPool;
//pointer to current pool page. equal to address of default pool at start
NodePoolPage* currentPoolPage;
//pointer to first node of linked list of nodes
Node* freeNodesList;
void InitPool() {
currentPoolPage=&defaultPool;
freeNodesList=0;
NodePoolPage* ptr=currentPoolPage;
while(ptr) {
ptr->Init();
ptr=ptr->nextPage;
}
}
// State
SearchState m_State;
@ -938,20 +969,8 @@ private: // data
// Start and goal state pointers
Node *m_Start;
Node *m_Goal;
Node *m_CurrentSolutionNode;
// Memory
FixedSizeAllocator<Node> m_FixedSizeAllocator;
//Debug : need to keep these two iterators around
// for the user Dbg functions
typename vector< Node * >::iterator iterDbgOpen;
typename vector< Node * >::iterator iterDbgClosed;
// debugging : count memory allocation and free's
int m_AllocateNodeCount;
bool m_CancelRequest;
void *userData;
@ -975,7 +994,8 @@ public:
bool IsGoal( MapSearchNode &nodeGoal );
bool GetSuccessors( AStarSearch<MapSearchNode> *astarsearch, MapSearchNode *parent_node );
float GetCost( MapSearchNode &successor, void *userData );
bool IsSameState( MapSearchNode &rhs );
bool IsSameState( const MapSearchNode &rhs ) const;
int32 getHashCode() const;
void PrintNodeInfo();
@ -986,27 +1006,31 @@ private:
AI_Node *mNode;
};
bool MapSearchNode::IsSameState( MapSearchNode &rhs ) {
inline bool MapSearchNode::IsSameState( const MapSearchNode &rhs ) const {
bool ret = false;
if ( mNode == rhs.mNode ) ret = true;
return ret;
}
inline int32 MapSearchNode::getHashCode() const {
return mNode->getHashCode();
}
void MapSearchNode::PrintNodeInfo() {
}
float MapSearchNode::GoalDistanceEstimate( MapSearchNode &nodeGoal, void *userData ) {
inline float MapSearchNode::GoalDistanceEstimate( MapSearchNode &nodeGoal, void *userData ) {
return mNode->getDistance( nodeGoal.mNode, userData );
}
bool MapSearchNode::IsGoal( MapSearchNode &nodeGoal ) {
inline bool MapSearchNode::IsGoal( MapSearchNode &nodeGoal ) {
bool ret = false;
if ( mNode == nodeGoal.mNode ) ret = true;
return ret;
}
bool MapSearchNode::GetSuccessors( AStarSearch<MapSearchNode> *astarsearch, MapSearchNode *parent_node ) {
inline bool MapSearchNode::GetSuccessors( AStarSearch<MapSearchNode> *astarsearch, MapSearchNode *parent_node ) {
unsigned int count = mNode->getEdgeCount(astarsearch->getUserData());
for (unsigned int i = 0; i < count; ++i) {
AI_Node *node = mNode->getEdge(i,astarsearch->getUserData());
@ -1017,7 +1041,7 @@ bool MapSearchNode::GetSuccessors( AStarSearch<MapSearchNode> *astarsearch, MapS
return true;
}
float MapSearchNode::GetCost( MapSearchNode &successor, void *userData ) {
inline float MapSearchNode::GetCost( MapSearchNode &successor, void *userData ) {
return mNode->getCost(userData);
}

View File

@ -115,7 +115,9 @@ Copyright (C)2001-2005 Justin Heyes-Jones
//** releaseFastAstar(fa);
//**
//*******************************
#include "data_types.h"
using namespace Shared::Platform;
class AI_Node
{
@ -124,6 +126,7 @@ public:
virtual float getCost(void *userData) = 0;
virtual unsigned int getEdgeCount(void *userData) const = 0;
virtual AI_Node * getEdge(int index,void *userData) const = 0;
virtual int32 getHashCode() const = 0;
};
enum SearchState {

View File

@ -28,6 +28,7 @@
#include "unit_type.h"
#include "fast_path_finder.h"
#include "command.h"
#include "checksum.h"
#include "leak_dumper.h"
@ -207,6 +208,7 @@ class FastAINode : public AI_Node {
protected:
Vec2i pos;
const Map *map;
int32 hashCode;
static const int NODE_EDGE_COUNT = 8;
FastAINode * getNodeForEdgeIndex(int index,void *userData) const;
@ -219,17 +221,26 @@ public:
FastAINode(Vec2i &pos,const Map *map) {
this->pos = pos;
this->map = map;
Checksum result;
result.addInt(pos.x);
result.addInt(pos.y);
hashCode = result.getSum();
}
void setData(Vec2i pos, const Map *map) {
this->pos = pos;
this->map = map;
Checksum result;
result.addInt(pos.x);
result.addInt(pos.y);
hashCode = result.getSum();
}
inline const Vec2i & getPos() const { return pos; }
virtual float getDistance(const AI_Node *node, void *userData);
virtual float getCost(void *userData);
virtual float getDistance(const AI_Node *node, void *userData);
virtual float getCost(void *userData);
virtual unsigned int getEdgeCount(void *userData) const;
virtual AI_Node * getEdge(int index, void *userData) const;
virtual AI_Node * getEdge(int index, void *userData) const;
inline virtual int32 getHashCode() const { return hashCode; }
};
class Map {