AnotherFoxGuy/polyvox
1
0
Fork 0
Code Issues Pull Requests Activity

Moved AStarPathfinder code from Thermite to PolyVox.

Browse Source
This commit is contained in:
David Williams 2010-12-08 23:06:29 +00:00
parent 30425a4d86
commit c9331c3e35
5 changed files with 748 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
library/PolyVoxCore/CMakeLists.txt
View File

@ -5,6 +5,7 @@ PROJECT(PolyVoxCore)
#Projects source files
SET(CORE_SRC_FILES
source/ArraySizes.cpp
source/AStarPathfinder.cpp
source/GradientEstimators.cpp
source/Log.cpp
source/Mesh.cpp
@ -21,6 +22,8 @@ SET(CORE_INC_FILES
include/Array.h
include/Array.inl
include/ArraySizes.h
include/AStarPathfinder.h
include/AStarPathfinder.inl
include/CubicSurfaceExtractor.h
include/CubicSurfaceExtractor.inl
include/CubicSurfaceExtractorWithNormals.h
@ -60,6 +63,7 @@ SET(IMPL_SRC_FILES
SET(IMPL_INC_FILES
include/PolyVoxImpl/ArraySizesImpl.h
include/PolyVoxImpl/ArraySizesImpl.inl
include/PolyVoxImpl/AStarPathfinderImpl.h
include/PolyVoxImpl/Block.h
include/PolyVoxImpl/Block.inl
include/PolyVoxImpl/MarchingCubesTables.h

135
library/PolyVoxCore/include/AStarPathfinder.h Normal file
View File

@ -0,0 +1,135 @@
/*******************************************************************************
Copyright (c) 2005-2009 David Williams
This software is provided 'as-is', without any express or implied
warranty. In no event will the authors be held liable for any damages
arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it
freely, subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not
claim that you wrote the original software. If you use this software
in a product, an acknowledgment in the product documentation would be
appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be
misrepresented as being the original software.
3. This notice may not be removed or altered from any source
distribution.
*******************************************************************************/
#ifndef __PolyVox_AStarPathfinder_H__
#define __PolyVox_AStarPathfinder_H__
#include "Array.h"
#include "PolyVoxImpl/AStarPathfinderImpl.h"
#include "PolyVoxForwardDeclarations.h"
#include "Volume.h"
#include "VolumeSampler.h"
#include "PolyVoxImpl/TypeDef.h"
#include <functional>
namespace PolyVox
{
const float sqrt_1 = 1.0f;
const float sqrt_2 = 1.4143f;
const float sqrt_3 = 1.7321f;
extern const POLYVOXCORE_API Vector3DInt16 arrayPathfinderFaces[6];
extern const POLYVOXCORE_API Vector3DInt16 arrayPathfinderEdges[12];
extern const POLYVOXCORE_API Vector3DInt16 arrayPathfinderCorners[8];
template <typename VoxelType>
bool aStarDefaultVoxelValidator(const Volume<VoxelType>* volData, const Vector3DInt16& v3dPos);
template <typename VoxelType>
struct AStarPathfinderParams
{
public:
AStarPathfinderParams
(
Volume<VoxelType>* volData,
const Vector3DInt16& v3dStart,
const Vector3DInt16& v3dEnd,
std::list<Vector3DInt16>* listResult,
float fHBias = 1.0,
uint32_t uMaxNoOfNodes = 10000,
Connectivity connectivity = TwentySixConnected,
std::function<bool (const Volume<VoxelType>*, const Vector3DInt16&)> funcIsVoxelValidForPath = &aStarDefaultVoxelValidator<VoxelType>,
std::function<void (float)> funcProgressCallback = 0
)
:volume(volData)
,start(v3dStart)
,end(v3dEnd)
,result(listResult)
,hBias(fHBias)
,connectivity(connectivity)
,isVoxelValidForPath(funcIsVoxelValidForPath)
,maxNumberOfNodes(uMaxNoOfNodes)
,progressCallback(funcProgressCallback)
{
}
//The volume data.
Volume<VoxelType>* volume;
Vector3DInt16 start;
Vector3DInt16 end;
//The resulting path
std::list<Vector3DInt16>* result;
//The requied connectivity
Connectivity connectivity;
//Bias applied to h()
float hBias;
//Max number of nodes to examine
uint32_t maxNumberOfNodes;
//Used to determine whether a given voxel is valid.
std::function<bool (const Volume<VoxelType>*, const Vector3DInt16&)> isVoxelValidForPath;
//Progress callback
std::function<void (float)> progressCallback;
};
template <typename VoxelType>
class AStarPathfinder
{
public:
AStarPathfinder(const AStarPathfinderParams<VoxelType>& params);
void execute();
private:
void processNeighbour(const Vector3DInt16& neighbourPos, float neighbourGVal);
float SixConnectedCost(const Vector3DInt16& a, const Vector3DInt16& b);
float EighteenConnectedCost(const Vector3DInt16& a, const Vector3DInt16& b);
float TwentySixConnectedCost(const Vector3DInt16& a, const Vector3DInt16& b);
float computeH(const Vector3DInt16& a, const Vector3DInt16& b);
//Node containers
AllNodesContainer allNodes;
OpenNodesContainer openNodes;
ClosedNodesContainer closedNodes;
//The current node
AllNodesContainer::iterator current;
float m_fProgress;
AStarPathfinderParams<VoxelType> m_params;
};
}
#include "AStarPathfinder.inl"
#endif //__PolyVox_AStarPathfinder_H__

322
library/PolyVoxCore/include/AStarPathfinder.inl Normal file
View File

@ -0,0 +1,322 @@
/*******************************************************************************
Copyright (c) 2005-2009 David Williams
This software is provided 'as-is', without any express or implied
warranty. In no event will the authors be held liable for any damages
arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it
freely, subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not
claim that you wrote the original software. If you use this software
in a product, an acknowledgment in the product documentation would be
appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be
misrepresented as being the original software.
3. This notice may not be removed or altered from any source
distribution.
*******************************************************************************/
#include <cfloat> //For numeric_limits
namespace PolyVox
{
////////////////////////////////////////////////////////////////////////////////
// aStarDefaultVoxelValidator free function
////////////////////////////////////////////////////////////////////////////////
template <typename VoxelType>
bool aStarDefaultVoxelValidator(const Volume<VoxelType>* volData, const Vector3DInt16& v3dPos)
{
//Voxels are considered valid candidates for the path if they are inside the volume...
if(volData->getEnclosingRegion().containsPoint(v3dPos) == false)
{
return false;
}
//and if their density is below the threshold.
Material8 voxel = volData->getVoxelAt(static_cast<Vector3DUint16>(v3dPos));
if(voxel.getDensity() >= Material8::getThreshold())
{
return false;
}
return true;
}
////////////////////////////////////////////////////////////////////////////////
// AStarPathfinder Class
////////////////////////////////////////////////////////////////////////////////
template <typename VoxelType>
AStarPathfinder<VoxelType>::AStarPathfinder(const AStarPathfinderParams<VoxelType>& params)
:m_params(params)
{
}
template <typename VoxelType>
void AStarPathfinder<VoxelType>::execute()
{
//Clear any existing nodes
allNodes.clear();
openNodes.clear();
closedNodes.clear();
//Clear the result
m_params.result->clear();
AllNodesContainer::iterator startNode = allNodes.insert(Node(m_params.start.getX(), m_params.start.getY(), m_params.start.getZ())).first;
AllNodesContainer::iterator endNode = allNodes.insert(Node(m_params.end.getX(), m_params.end.getY(), m_params.end.getZ())).first;
/*Node::startPos = startNode->position;
Node::endPos = endNode->position;
Node::m_eConnectivity = m_eConnectivity;*/
Node* tempStart = const_cast<Node*>(&(*startNode));
tempStart->gVal = 0;
tempStart->hVal = computeH(startNode->position, endNode->position);
Node* tempEnd = const_cast<Node*>(&(*endNode));
tempEnd->hVal = 0.0f;
openNodes.insert(startNode);
float fDistStartToEnd = (endNode->position - startNode->position).length();
m_fProgress = 0.0f;
if(m_params.progressCallback)
{
m_params.progressCallback(m_fProgress);
}
while((openNodes.empty() == false) && (openNodes.getFirst() != endNode))
{
//Move the first node from open to closed.
current = openNodes.getFirst();
openNodes.removeFirst();
closedNodes.insert(current);
//Update the user on our progress
if(m_params.progressCallback)
{
const float fMinProgresIncreament = 0.001f;
float fDistCurrentToEnd = (endNode->position - current->position).length();
float fDistNormalised = fDistCurrentToEnd / fDistStartToEnd;
float fProgress = 1.0f - fDistNormalised;
if(fProgress >= m_fProgress + fMinProgresIncreament)
{
m_fProgress = fProgress;
m_params.progressCallback(m_fProgress);
}
}
//The distance from one cell to another connected by face, edge, or corner.
const float fFaceCost = sqrt_1;
const float fEdgeCost = sqrt_2;
const float fCornerCost = sqrt_3;
//Process the neighbours. Note the deliberate lack of 'break'
//statements, larger connectivities include smaller ones.
switch(m_params.connectivity)
{
case TwentySixConnected:
processNeighbour(current->position + arrayPathfinderCorners[0], current->gVal + fCornerCost);
processNeighbour(current->position + arrayPathfinderCorners[1], current->gVal + fCornerCost);
processNeighbour(current->position + arrayPathfinderCorners[2], current->gVal + fCornerCost);
processNeighbour(current->position + arrayPathfinderCorners[3], current->gVal + fCornerCost);
processNeighbour(current->position + arrayPathfinderCorners[4], current->gVal + fCornerCost);
processNeighbour(current->position + arrayPathfinderCorners[5], current->gVal + fCornerCost);
processNeighbour(current->position + arrayPathfinderCorners[6], current->gVal + fCornerCost);
processNeighbour(current->position + arrayPathfinderCorners[7], current->gVal + fCornerCost);
case EighteenConnected:
processNeighbour(current->position + arrayPathfinderEdges[ 0], current->gVal + fEdgeCost);
processNeighbour(current->position + arrayPathfinderEdges[ 1], current->gVal + fEdgeCost);
processNeighbour(current->position + arrayPathfinderEdges[ 2], current->gVal + fEdgeCost);
processNeighbour(current->position + arrayPathfinderEdges[ 3], current->gVal + fEdgeCost);
processNeighbour(current->position + arrayPathfinderEdges[ 4], current->gVal + fEdgeCost);
processNeighbour(current->position + arrayPathfinderEdges[ 5], current->gVal + fEdgeCost);
processNeighbour(current->position + arrayPathfinderEdges[ 6], current->gVal + fEdgeCost);
processNeighbour(current->position + arrayPathfinderEdges[ 7], current->gVal + fEdgeCost);
processNeighbour(current->position + arrayPathfinderEdges[ 8], current->gVal + fEdgeCost);
processNeighbour(current->position + arrayPathfinderEdges[ 9], current->gVal + fEdgeCost);
processNeighbour(current->position + arrayPathfinderEdges[10], current->gVal + fEdgeCost);
processNeighbour(current->position + arrayPathfinderEdges[11], current->gVal + fEdgeCost);
case SixConnected:
processNeighbour(current->position + arrayPathfinderFaces[0], current->gVal + fFaceCost);
processNeighbour(current->position + arrayPathfinderFaces[1], current->gVal + fFaceCost);
processNeighbour(current->position + arrayPathfinderFaces[2], current->gVal + fFaceCost);
processNeighbour(current->position + arrayPathfinderFaces[3], current->gVal + fFaceCost);
processNeighbour(current->position + arrayPathfinderFaces[4], current->gVal + fFaceCost);
processNeighbour(current->position + arrayPathfinderFaces[5], current->gVal + fFaceCost);
}
if(allNodes.size() > m_params.maxNumberOfNodes)
{
//We've reached the specified maximum number
//of nodes. Just give up on the search.
break;
}
}
if((openNodes.empty()) || (openNodes.getFirst() != endNode))
{
//In this case we failed to find a valid path.
throw runtime_error("No path found");
}
else
{
Node* n = const_cast<Node*>(&(*endNode));
while(n != 0)
{
m_params.result->push_front(n->position);
n = n->parent;
}
}
if(m_params.progressCallback)
{
m_params.progressCallback(1.0f);
}
}
template <typename VoxelType>
void AStarPathfinder<VoxelType>::processNeighbour(const Vector3DInt16& neighbourPos, float neighbourGVal)
{
bool bIsVoxelValidForPath = m_params.isVoxelValidForPath(m_params.volume, neighbourPos);
if(!bIsVoxelValidForPath)
{
return;
}
float cost = neighbourGVal;
std::pair<AllNodesContainer::iterator, bool> insertResult = allNodes.insert(Node(neighbourPos.getX(), neighbourPos.getY(), neighbourPos.getZ()));
AllNodesContainer::iterator neighbour = insertResult.first;
if(insertResult.second == true) //New node, compute h.
{
Node* tempNeighbour = const_cast<Node*>(&(*neighbour));
tempNeighbour -> hVal = computeH(neighbour->position, m_params.end);
}
OpenNodesContainer::iterator openIter = openNodes.find(neighbour);
if(openIter != openNodes.end())
{
if(cost < neighbour->gVal)
{
openNodes.remove(openIter);
openIter = openNodes.end();
}
}
//TODO - Nodes could keep track of if they are in open or closed? And a pointer to where they are?
ClosedNodesContainer::iterator closedIter = closedNodes.find(neighbour);
if(closedIter != closedNodes.end())
{
if(cost < neighbour->gVal)
{
//Probably shouldn't happen?
closedNodes.remove(closedIter);
closedIter = closedNodes.end();
}
}
if((openIter == openNodes.end()) && (closedIter == closedNodes.end()))
{
Node* temp = const_cast<Node*>(&(*neighbour));
temp->gVal = cost;
openNodes.insert(neighbour);
temp->parent = const_cast<Node*>(&(*current));
}
}
template <typename VoxelType>
float AStarPathfinder<VoxelType>::SixConnectedCost(const Vector3DInt16& a, const Vector3DInt16& b)
{
//This is the only heuristic I'm sure of - just use the manhatten distance for the 6-connected case.
uint16_t faceSteps = abs(a.getX()-b.getX()) + abs(a.getY()-b.getY()) + abs(a.getZ()-b.getZ());
return faceSteps * 1.0f;
}
template <typename VoxelType>
float AStarPathfinder<VoxelType>::EighteenConnectedCost(const Vector3DInt16& a, const Vector3DInt16& b)
{
//I'm not sure of the correct heuristic for the 18-connected case, so I'm just letting it fall through to the
//6-connected case. This means 'h' will be bigger than it should be, resulting in a faster path which may not
//actually be the shortest one. If you have a correct heuristic for the 18-connected case then please let me know.
return SixConnectedCost(a,b);
}
template <typename VoxelType>
float AStarPathfinder<VoxelType>::TwentySixConnectedCost(const Vector3DInt16& a, const Vector3DInt16& b)
{
//Can't say I'm certain about this heuristic - if anyone has
//a better idea of what it should be then please let me know.
uint16_t array[3];
array[0] = abs(a.getX() - b.getX());
array[1] = abs(a.getY() - b.getY());
array[2] = abs(a.getZ() - b.getZ());
std::sort(&array[0], &array[3]);
uint16_t cornerSteps = array[0];
uint16_t edgeSteps = array[1] - array[0];
uint16_t faceSteps = array[2] - array[1];
return cornerSteps * sqrt_3 + edgeSteps * sqrt_2 + faceSteps * sqrt_1;
}
template <typename VoxelType>
float AStarPathfinder<VoxelType>::computeH(const Vector3DInt16& a, const Vector3DInt16& b)
{
float hVal;
switch(m_params.connectivity)
{
case TwentySixConnected:
hVal = TwentySixConnectedCost(a, b);
break;
case EighteenConnected:
hVal = EighteenConnectedCost(a, b);
break;
case SixConnected:
hVal = SixConnectedCost(a, b);
break;
default:
assert(false); //Invalid case.
}
//Sanity checks in debug mode. These can come out eventually, but I
//want to make sure that the heuristics I've come up with make sense.
assert((a-b).length() <= TwentySixConnectedCost(a,b));
assert(TwentySixConnectedCost(a,b) <= EighteenConnectedCost(a,b));
assert(EighteenConnectedCost(a,b) <= SixConnectedCost(a,b));
//Apply the bias to the computed h value;
hVal *= m_params.hBias;
std::hash<uint32_t> uint32Hash;
uint32_t hashValX = uint32Hash(a.getX());
uint32_t hashValY = uint32Hash(a.getY());
uint32_t hashValZ = uint32Hash(a.getZ());
uint32_t hashVal = hashValX ^ hashValY ^ hashValZ;
hashVal &= 0x0000FFFF;
float fHash = hashVal / 1000000.0f;
hVal += fHash;
return hVal;
}
}

218
library/PolyVoxCore/include/PolyVoxImpl/AStarPathfinderImpl.h Normal file
View File

@ -0,0 +1,218 @@
/*******************************************************************************
Copyright (c) 2005-2009 David Williams
This software is provided 'as-is', without any express or implied
warranty. In no event will the authors be held liable for any damages
arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it
freely, subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not
claim that you wrote the original software. If you use this software
in a product, an acknowledgment in the product documentation would be
appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be
misrepresented as being the original software.
3. This notice may not be removed or altered from any source
distribution.
*******************************************************************************/
#ifndef __PolyVox_AStarPathfinderImpl_H__
#define __PolyVox_AStarPathfinderImpl_H__
#include "Vector.h"
#include <set>
#include <vector>
namespace PolyVox
{
class OpenNodesContainer;
class ClosedNodesContainer;
class ThermiteGameLogic;
enum Connectivity
{
SixConnected,
EighteenConnected,
TwentySixConnected
};
struct Node
{
Node(int x, int y, int z)
:gVal(std::numeric_limits<float>::quiet_NaN()) //Initilise with NaNs so that we will
,hVal(std::numeric_limits<float>::quiet_NaN()) //know if we forget to set these properly.
,parent(0)
{
position.setX(x);
position.setY(y);
position.setZ(z);
}
bool operator==(const Node& rhs) const
{
return position == rhs.position;
}
bool operator<(const Node& rhs) const
{
if (position.getX() < rhs.position.getX())
return true;
if (rhs.position.getX() < position.getX())
return false;
if (position.getY() < rhs.position.getY())
return true;
if (rhs.position.getY() < position.getY())
return false;
if (position.getZ() < rhs.position.getZ())
return true;
if (rhs.position.getZ() < position.getZ())
return false;
return false;
}
PolyVox::Vector3DInt16 position;
Node* parent;
float gVal;
float hVal;
float f(void) const
{
float f = gVal + hVal;
return f;
}
};
typedef std::set<Node> AllNodesContainer;
class AllNodesContainerIteratorComparator
{
public:
bool operator() (const AllNodesContainer::iterator& lhs, const AllNodesContainer::iterator& rhs) const
{
return (&(*lhs)) < (&(*rhs));
}
};
class NodeSort
{
public:
bool operator() (const AllNodesContainer::iterator& lhs, const AllNodesContainer::iterator& rhs) const
{
return lhs->f() > rhs->f();
}
};
class OpenNodesContainer
{
public:
typedef std::vector<AllNodesContainer::iterator>::iterator iterator;
public:
void clear(void)
{
open.clear();
}
bool empty(void) const
{
return open.empty();
}
void insert(AllNodesContainer::iterator node)
{
open.push_back(node);
push_heap(open.begin(), open.end(), NodeSort());
}
AllNodesContainer::iterator getFirst(void)
{
return open[0];
}
void removeFirst(void)
{
pop_heap(open.begin(), open.end(), NodeSort());
open.pop_back();
}
void remove(iterator iterToRemove)
{
open.erase(iterToRemove);
make_heap(open.begin(), open.end(), NodeSort());
}
iterator begin(void)
{
return open.begin();
}
iterator end(void)
{
return open.end();
}
iterator find(AllNodesContainer::iterator node)
{
std::vector<AllNodesContainer::iterator>::iterator openIter = std::find(open.begin(), open.end(), node);
return openIter;
}
private:
std::vector<AllNodesContainer::iterator> open;
};
class ClosedNodesContainer
{
public:
typedef std::set<AllNodesContainer::iterator, AllNodesContainerIteratorComparator>::iterator iterator;
public:
void clear(void)
{
closed.clear();
}
void insert(AllNodesContainer::iterator node)
{
closed.insert(node);
}
void remove(iterator iterToRemove)
{
closed.erase(iterToRemove);
}
iterator begin(void)
{
return closed.begin();
}
iterator end(void)
{
return closed.end();
}
iterator find(AllNodesContainer::iterator node)
{
iterator iter = std::find(closed.begin(), closed.end(), node);
return iter;
}
private:
std::set<AllNodesContainer::iterator, AllNodesContainerIteratorComparator> closed;
};
//bool operator<(const AllNodesContainer::iterator& lhs, const AllNodesContainer::iterator& rhs);
}
#endif //__PolyVox_AStarPathfinderImpl_H__

69
library/PolyVoxCore/source/AStarPathfinder.cpp Normal file
View File

@ -0,0 +1,69 @@
/*******************************************************************************
Copyright (c) 2005-2009 David Williams
This software is provided 'as-is', without any express or implied
warranty. In no event will the authors be held liable for any damages
arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it
freely, subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not
claim that you wrote the original software. If you use this software
in a product, an acknowledgment in the product documentation would be
appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be
misrepresented as being the original software.
3. This notice may not be removed or altered from any source
distribution.
*******************************************************************************/
#include "AStarPathfinder.h"
#include "Material.h"
using namespace PolyVox;
namespace PolyVox
{
const Vector3DInt16 arrayPathfinderFaces[6] =
{
Vector3DInt16(0, 0, -1),
Vector3DInt16(0, 0, +1),
Vector3DInt16(0, -1, 0),
Vector3DInt16(0, +1, 0),
Vector3DInt16(-1, 0, 0),
Vector3DInt16(+1, 0, 0)
};
const Vector3DInt16 arrayPathfinderEdges[12] =
{
Vector3DInt16(0, -1, -1),
Vector3DInt16(0, -1, +1),
Vector3DInt16(0, +1, -1),
Vector3DInt16(0, +1, +1),
Vector3DInt16(-1, 0, -1),
Vector3DInt16(-1, 0, +1),
Vector3DInt16(+1, 0, -1),
Vector3DInt16(+1, 0, +1),
Vector3DInt16(-1, -1, 0),
Vector3DInt16(-1, +1, 0),
Vector3DInt16(+1, -1, 0),
Vector3DInt16(+1, +1, 0)
};
const Vector3DInt16 arrayPathfinderCorners[8] =
{
Vector3DInt16(-1, -1, -1),
Vector3DInt16(-1, -1, +1),
Vector3DInt16(-1, +1, -1),
Vector3DInt16(-1, +1, +1),
Vector3DInt16(+1, -1, -1),
Vector3DInt16(+1, -1, +1),
Vector3DInt16(+1, +1, -1),
Vector3DInt16(+1, +1, +1)
};
}