A* pathfinding routine

[Toupie]

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

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


[Toupie]

Here is an example script that uses AStar.nut to walk a programmable robot to a point.

The goal is defined by:

Code: [Select]

DestX <- 44;
DestY <- 0;
DestZ <- 11;
You can place the robot anywhere and it will try and find it's way to that location in the shortest possible route without going through any solid blocks.

Example Code:

Code: [Select]

// Test of pathfinding

Time <- 0;

DestX <- 44;
DestY <- 0;
DestZ <- 11;
MovePath <- null;
MyAstar <- null;

function Voxel_Load()
{
  Time = GetGameTime();
  log_info("Voxel_Load()");
  dofile(GetPath(3)+GetInfo(20)+"AStar.nut",true);
  MyAstar = AStar(); // Create instance of AStar
  //MyAstar.SetMax(32); // Limit search area
  MyAstar.Set3D();  // Enable full 3D search
}

function Voxel_Step()
{
  if (GetGameTime() - Time > 0) {
    Time = GetGameTime();
  } else {
    // Fix for funky GetGameTime()
    if (GetGameTime() - Time < -60000)
      Time = GetGameTime();
    return;
  }

  if (MoveTo(DestX,DestY,DestZ)) {
    if (GetX() == DestX && GetY() == DestY && GetZ() == DestZ) {
      log_info("We have arrived!");
      Time = GetGameTime() + 10000;
    } else {
      log_err("Failed to arrive");
      Time = GetGameTime() + 10000;
    }
  }
}

function MoveTo(x, y, z)
{
  if (MovePath == null) { // We need to keep calling GetPath until we get an array back.
    MovePath = MyAstar.GetPath(Node(GetX(),GetY(),GetZ()),Node(x,y,z));
  }
  if (MovePath != null && MovePath.len()>0) {
    local d = MovePath.pop(); // Get move direction from array
    if (GetVoxelProp(Look(d),0)) {
      if (Move(d)) {
        if (MovePath.len() == 0)
          return true; // We should have arrived.
      } else {
        MovePath.push(d); // Save move direction to path again.
        log_err("Failed to move along the path! d="+d);
        Time = GetGameTime() + 5000;
      }
    } else {
      log_err("Path is obstructed. Getting new move path!");
      Time = GetGameTime() + 5000;
      MyAstar.Clear();
      MovePath = null;
    }
  } else if (typeof MovePath=="array" && MovePath.len()==0) {
    return true; // We did not arrive, but we still have to tell the caller we are done.
  }
  return false; // Not there yet.
}


function log_info(msg)
{
    local s;
    s = format("5.nut at %d,%d,%d Info: %s", GetX(), GetY(), GetZ(), msg);
    Display( s, 2000, 4, 1000);
    error(s+"\n");
}

function log_err(msg)
{
    local s;
    s = format("5.nut at %d,%d,%d Err: %s", GetX(), GetY(), GetZ(), msg);
    Display( s, 4000, 4, 4000);
    error(s+"\n");
    Time = Time + 1500;
}
It's the function MoveTo(x,y,z) that is the meat of the example.

[olive]

Amazing program. And a really cool example of implementation of this kind of algorithm.

That's also a good way for beginners to understand problems in algorithmic. Particularly the notion of complexity because games show importance of execution time.

[Qon]

A nice programming exercise but not very useful atm. For O(1) running time and movement time

Code: [Select]

MoveVoxel(0,0,0, destX-GetX(),destY-GetY(),destZ-GetZ()) But I get that wasn't the point of this thread. If there's a qomputer tier in the future that has Look3D() but not MoveVoxel3D() then this will be handy.