- improved the new pathfinder using hashmap
This commit is contained in:
parent
f79222defa
commit
1b2b1f9821
|
@ -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
|
|
@ -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
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
@ -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 {
|
||||
|
|
|
@ -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 {
|
||||
|
|
Loading…
Reference in New Issue