1
Programming with Blackvoxel / A* pathfinding routine
« on: October 31, 2013, 10:41:38 pm »
Now that we have the 3D functions I thought that I could write me a path finding routine. So here it is. It's a basic A* implementation that is configurable between 2D (XZ) and 3D (XYZ).
A little warning. 3D path finding over large distances or in complex scenarios can be very slow.
I've implemented it as 2 classes.
AStar is the real code of the path finding.
Node is just a simple container for a node in the path. It is used by AStar
Node(x,y,z) creates an instance of Node for that location.
AStar() creates an instance of AStar.
Methods of AStar
result = GetPath( start, goal )
start and goal are instances of Node(x,y,z) representing the start position and the goal of the path.
result will be an array containing the necessary move directions you need to take to get from start to goal. The moves are stored in reverse order so you can just .pop() them from back of the array. GetPath will limit it's processing time to 200ms for every call to it, and will return null until it is done. So you will have to call it several times before getting your path. In case it fails to find a path to goal it will return an empty array.
Clear()
Resets the instance of AStar for a new search. Needs to be called after you have done a search before you start a new one. Can be used instead of creating a new instance of AStar.
Set2d()
Sets the search mode to 2D (XZ), ignoring Y. This is the default.
Set3D()
Sets the search mode to full 3D. Warning, can be slow.
SetMax(value)
Sets the limits of the search to value in all directions (xyz) from the center point between start and goal. Default values are 64, which gives it an area of 128*128 or 128*128*128 in 3D that will be used when searching for a path. This is necessary, or GetPath would never give up in case there is no path between start and goal.
Members of AStar
xmax, ymax, zmax
The limits on the 3 coordinate for the search. Default value 64.
debug
How much will be printed out to stderr.txt. 0 = nothing. 1 = summary. 2 = detail information (not implemented since it slows things down to much)
AStar.nut
A little warning. 3D path finding over large distances or in complex scenarios can be very slow.
I've implemented it as 2 classes.
AStar is the real code of the path finding.
Node is just a simple container for a node in the path. It is used by AStar
Node(x,y,z) creates an instance of Node for that location.
AStar() creates an instance of AStar.
Methods of AStar
result = GetPath( start, goal )
start and goal are instances of Node(x,y,z) representing the start position and the goal of the path.
result will be an array containing the necessary move directions you need to take to get from start to goal. The moves are stored in reverse order so you can just .pop() them from back of the array. GetPath will limit it's processing time to 200ms for every call to it, and will return null until it is done. So you will have to call it several times before getting your path. In case it fails to find a path to goal it will return an empty array.
Clear()
Resets the instance of AStar for a new search. Needs to be called after you have done a search before you start a new one. Can be used instead of creating a new instance of AStar.
Set2d()
Sets the search mode to 2D (XZ), ignoring Y. This is the default.
Set3D()
Sets the search mode to full 3D. Warning, can be slow.
SetMax(value)
Sets the limits of the search to value in all directions (xyz) from the center point between start and goal. Default values are 64, which gives it an area of 128*128 or 128*128*128 in 3D that will be used when searching for a path. This is necessary, or GetPath would never give up in case there is no path between start and goal.
Members of AStar
xmax, ymax, zmax
The limits on the 3 coordinate for the search. Default value 64.
debug
How much will be printed out to stderr.txt. 0 = nothing. 1 = summary. 2 = detail information (not implemented since it slows things down to much)
AStar.nut
Code: [Select]
// A* pathfinding
class Node {
constructor(x,y,z)
{
this.x = x;
this.y = y;
this.z = z;
}
// This one does not seem to work.
function _cmp(other)
{
error("_cmp This.x="+x+"This.z="+z+" other.x="+other.x+" other.z="+other.z+"\n");
if (x < other.x) return -1;
if (y < other.y) return -1;
if (z < other.z) return -1;
if (x > other.x) return 1;
if (y > other.y) return 1;
if (z > other.z) return 1;
return 0;
}
function index()
{
return format("%d:%d:%d",x,y,z);
}
x = null; // X position
y = null; // Y position
z = null; // Z position
d = null; // Direction to get here;
g = 0; // Cost from start to here
h = 0; // Estimated cost from here to goal
f = 0; // Total estimated cost to goal
p = null; // Came from (parent). Used to walk back from goal to get the shortest path.
}
class AStar {
constructor()
{
open = {};
closed = {};
}
// Private Members
open = null; // Our open set of nodes
closed = null; // Our close set of nodes
checked = 0; // The number of nodes checked
time = 0; // Total time processed
directions = 4; // 4 = X & Z, 6 = X, Y & Z
moveCoord = [ [0,0,1], [1,0,0], [0,0,-1], [-1,0,0], [0,1,0], [0,-1,0] ];
// Public Members
// A full 3D scan of an area of the size [64*2,64*2,64*2] will take up to 10 minutes
xmax = 64;
ymax = 64;
zmax = 64;
debug = 1; // 0=nothing, 1=summary, 2=full debug info
// Private Methods
function CostEstimate(start,goal)
{
return (abs(goal.x-start.x)+abs(goal.y-start.y)+abs(goal.z-start.z)) * 10;
}
function GetCheapest()
{
local m=null;
local i,v;
foreach (i,v in open) {
if (m==null || v.f<m.f) {
m = v;
}
}
delete open[m.index()];
return m;
}
function ReconstructPath(n)
{
local path = [];
while( n.p!=null ) {
path.append(n.d);
n = n.p;
}
if (debug>0) error("PathLen="+path.len()+"\n");
return path;
}
// Public Methods
function Clear()
{
closed = {};
open = {};
checked = 0;
time = 0;
}
function GetPath(start,goal)
{
local i,x,y,z,v;
local time = GetGameTime();
local midx = (goal.x-start.x)/2+start.x;
local midy = (goal.y-start.y)/2+start.y;
local midz = (goal.z-start.z)/2+start.z;
if (open.len()==0) {
start.g = 0;
start.h = CostEstimate(start,goal);
start.f = start.h;
open[start.index()] <- start; // open.push(start);
}
while (open.len() > 0) {
local current = GetCheapest();
if (current.x == goal.x && current.z == goal.z && (current.y == goal.y || directions == 4)) {
this.time += GetGameTime()-time;
if (debug>0) error("Found path to goal after "+checked+" passes and "+(this.time)+"ms!\n");
return ReconstructPath(current);
}
closed[current.index()] <- current; // closed.push(current);
//error(format("Check %d,%d,%d g=%d h=%d",current.x,current.y,current.z,current.g,current.h));
for (i=0; i<this.directions; i++) {
x = current.x+moveCoord[i][0];
y = current.y+moveCoord[i][1];
z = current.z+moveCoord[i][2];
if (abs(x-midx) <= xmax) {
if (abs(y-midy) <= ymax) {
if (abs(z-midz) <= zmax) {
v = Look3D(x-GetX(),y-GetY(),z-GetZ());
if (GetVoxelProp(v,0)) {
local neighbor = Node(x, y, z);
if (!(neighbor.index() in closed)) {
if (!(neighbor.index() in open)) {
neighbor.d = i;
neighbor.g = current.g + 10;
neighbor.h = CostEstimate(neighbor,goal);
neighbor.f = neighbor.g + neighbor.h;
neighbor.p = current;
//error(format(" | added %d,%d,%d g=%d h=%d",neighbor.x,neighbor.y,neighbor.z,neighbor.g,neighbor.h));
open[neighbor.index()] <- neighbor; // open.push(neighbor);
}
}
}
}
}
}
}
//error("\n");
checked++;
if (GetGameTime()-time>=200) { // Allow max 200ms in every call.
this.time += GetGameTime()-time;
if (debug>0) error("Yielded after "+checked+" passes and "+(this.time)+"ms!\n");
// Return to prevent Voxel_Step from taking to long.
// We will continue where we left of on the next call
return null; // Not finished yet.
}
}
if (debug>0) error("Did not find a path to goal! "+checked+" passes and "+(this.time)+"ms\n");
return []; // Not able to find a path to goal.
}
// Only search in X & Z. This is the default.
function Set2D()
{
this.directions = 4;
}
// Full 3D pathfinding. CAN BE VERY SLOW!
function Set3D()
{
this.directions = 6;
}
// Set all max limits to the same value.
function SetMax(value)
{
xmax = value;
ymax = value;
zmax = value;
}
} // class AStar