A* Path finding in Lua

pathfinding.c

/*
 * AStarfinding
 *
 * local path_find = require "pathfinding"
 *
 * local function neighbors(add, x, y, dist)
 *    add(x+1, y, hint, 1)
 * end
 *
 * local r = path_find(sx, sy, tx, ty, neighbors)
 */

#define LUA_LIB
#include <lua.h>
#include <lauxlib.h>

#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>

#define PF_STATE "pf.State"

#define PF_MIN_SIZE     16
#define PF_MAX_SIZET    (~(size_t)0 - 100)
#define PF_MAX_HEAP     16384

typedef struct pf_Point {
    size_t prev; /* prev point in path */
    int    x, y;
    float  dist, esti;
} pf_Point;

typedef struct pf_Entry {
    ptrdiff_t next;
    size_t    idx;
} pf_Entry;

typedef struct pf_Table {
    pf_Entry* hash;
    size_t    lastfree;
    size_t    size;
} pf_Table;

typedef struct pf_Heap {
    size_t* tree;
    size_t  size, maxsize;
    size_t  capacity;
} pf_Heap;

typedef struct pf_PointList {
    pf_Point* list;
    size_t    curr;
    size_t    size;
    size_t    capacity;
} pf_PointList;

typedef struct pf_State {
    lua_State* L;
    pf_PointList pts;
    pf_Heap      heap;
    pf_Table     table;
} pf_State;

/* point */

static void pfP_free(pf_State* S) { free(S->pts.list); }

static pf_Entry* pfT_newkey(pf_State* S, pf_Table* t, int x, int y);

static pf_Entry* pfT_hash(pf_Table* t, int x, int y)
{
    return &t->hash[(((size_t)x * 2654435761U + y) * 2654435761U) & (t->size - 1)];
}

static void pfT_clear(pf_State* S) {
    memset(S->table.hash, 0, S->table.size * sizeof(pf_Entry));
    S->table.lastfree = S->table.size;
}

static size_t pfT_resize(pf_State* S, pf_Table* t, size_t size) {
    pf_Table nt;
    size_t i, newsize = PF_MIN_SIZE;
    while (newsize < PF_MAX_SIZET / sizeof(pf_Point) && newsize < size)
        newsize <<= 1;
    if (newsize < size) return 0;
    nt.size = newsize;
    nt.lastfree = newsize;
    nt.hash = (pf_Entry*)malloc(nt.lastfree * sizeof(pf_Entry));
    if (nt.hash == NULL) return 0;
    memset(nt.hash, 0, nt.size * sizeof(pf_Entry));
    for (i = 0; i < t->size; ++i) {
        size_t idx = t->hash[i].idx;
        if (idx > 0) {
            pf_Point* p = &S->pts.list[idx - 1];
            pf_Entry* e = pfT_newkey(S, &nt, p->x, p->y);
            if (e == NULL) { free(nt.hash); return 0; }
            e->idx = idx;
        }
    }
    free(t->hash);
    *t = nt;
    return newsize;
}

static pf_Entry* pfT_newkey(pf_State* S, pf_Table* t, int x, int y) {
    pf_Entry* mp, * other, * next, * f = NULL;
    if (t->size == 0 && pfT_resize(S, t, (size_t)t->size * 2) == 0) return NULL;
    if ((mp = pfT_hash(t, x, y))->idx) {
        pf_Point* p = &S->pts.list[mp->idx - 1];
        while (t->lastfree > 0) {
            pf_Entry* cur = &t->hash[--t->lastfree];
            if (cur->idx == 0 && cur->next == 0) { f = cur; break; }
        }
        if (f == NULL) return pfT_resize(S, t, t->size * 2) ?
            pfT_newkey(S, t, x, y) : NULL;
        if ((other = pfT_hash(t, p->x, p->y)) != mp) {
            while ((next = other + other->next) != mp) other = next;
            other->next = f - other;
            *f = *mp;
            if (mp->next) f->next += mp - f, mp->next = 0;
        }
        else {
            if (mp->next) f->next = mp - f + mp->next;
            else assert(f->next == 0);
            mp->next = f - mp;
            mp = f;
        }
    }
    return mp;
}

static pf_Entry* pfT_get(pf_State* S, int x, int y) {
    pf_Entry* e;
    if (S->table.size == 0) return NULL;
    for (e = pfT_hash(&S->table, x, y);
        S->pts.list[e->idx - 1].x != x ||
        S->pts.list[e->idx - 1].y != y;
        e += e->next)
        if (e->next == 0) return NULL;
    return e;
}

static size_t pfP_get(pf_State* S, int x, int y) {
    pf_Entry* e = pfT_get(S, x, y);
    pf_Point* p;
    if (e && e->idx) return e->idx - 1;
    if (S->pts.size >= S->pts.capacity) {
        pf_Point* pts = NULL;
        size_t size = S->pts.capacity ? S->pts.capacity : PF_MIN_SIZE;
        while (size <= S->pts.size && size < PF_MAX_SIZET / 2)
            size *= 2;
        if (size < S->pts.size
            || !(pts = (pf_Point*)realloc(S->pts.list, size * sizeof(*pts))))
        {
            luaL_error(S->L, "out of memory"); return 0;
        }
        S->pts.list = pts;
        S->pts.capacity = size;
    }
    e = pfT_newkey(S, &S->table, x, y);
    if (e == NULL) { luaL_error(S->L, "out of memory"); return 0; }
    p = &S->pts.list[S->pts.size++];
    e->idx = S->pts.size;
    memset(p, 0, sizeof(*p));
    p->x = x, p->y = y, p->dist = INFINITY;
    return e->idx - 1;
}

/* heap */

static void pfH_free(pf_State* S) { free(S->heap.tree); }

static float pfH_pri(pf_State* S, size_t i) {
    return S->pts.list[S->heap.tree[i]].dist + S->pts.list[S->heap.tree[i]].esti;
}

static void pfH_push(pf_State* S, size_t pidx) {
    pf_Point *p = &S->pts.list[pidx];
    float pri = p->dist + p->esti;
    size_t i, c;
    if (S->heap.size >= S->heap.maxsize)
        luaL_error(S->L, "max heap size reached: %d", (int)S->heap.maxsize);
    if (S->heap.size >= S->heap.capacity) {
        size_t* tree = NULL;
        size_t size = S->heap.capacity ? S->heap.capacity : PF_MIN_SIZE;
        while (size <= S->heap.size && size < PF_MAX_SIZET / 2)
            size *= 2;
        if (size < S->heap.size
            || !(tree = (size_t*)realloc(S->heap.tree, size * sizeof(*tree))))
        {
            luaL_error(S->L, "out of memory"); return;
        }
        S->heap.tree = tree;
        S->heap.capacity = size;
    }
    i = S->heap.size++;
    while (c = (i - 1) >> 1, i > 0
        && pfH_pri(S, c) > pri
        && (S->heap.tree[i] = S->heap.tree[c], i = c))
        ;
    S->heap.tree[i] = p - S->pts.list;
}

static size_t pfH_pop(pf_State* S) {
    pf_Point* p = S->heap.size ? &S->pts.list[S->heap.tree[0]] : NULL;
    if (p) {
        size_t i = 0, k;
        float pri = pfH_pri(S, --S->heap.size);
        if (S->heap.size == 0) return p - S->pts.list;
        while ((k = i << 1 | 1) < S->heap.size
            && pri > pfH_pri(S,
                k += k + 1 < S->heap.size && pfH_pri(S, k) > pfH_pri(S, k + 1))
            && (S->heap.tree[i] = S->heap.tree[k], i = k))
            ;
        S->heap.tree[i] = S->heap.tree[S->heap.size];
    }
    return p - S->pts.list;
}

/* state */

static void pf_initstate(lua_State* L, pf_State* S) {
    memset(S, 0, sizeof(*S));
    S->L = L;
}

static void pf_freestate(pf_State* S) {
    pfP_free(S);
    pfH_free(S);
    pf_initstate(S->L, S);
}

static int Lpf_delstate(lua_State* L) {
    pf_State* S = (pf_State*)lua_touserdata(L, 1);
    if (S) pf_freestate(S);
    return 0;
}

static pf_State* pf_defaultstate(lua_State* L) {
    pf_State* S;
    if (lua_getfield(L, LUA_REGISTRYINDEX, PF_STATE) == LUA_TUSERDATA)
        (S = (pf_State*)lua_touserdata(L, -1))->L = L;
    else {
        S = (pf_State*)lua_newuserdata(L, sizeof(pf_State));
        pf_initstate(L, S);
        lua_newtable(L);
        lua_pushcfunction(L, Lpf_delstate);
        lua_setfield(L, -2, "__gc");
        lua_setmetatable(L, -2);
        lua_setfield(L, LUA_REGISTRYINDEX, PF_STATE);
    }
    lua_pop(L, 1);
    return S;
}

/* path finding */

static int Lpf_addpt(lua_State* L) {
    pf_State* S = pf_defaultstate(L);
    int   x     = (int)luaL_checkinteger(L, 1);
    int   y     = (int)luaL_checkinteger(L, 2);
    float esti  = (float)luaL_checknumber(L, 3);
    float dist  = (float)luaL_optnumber(L, 4, 1.f) + S->pts.list[S->pts.curr].dist;
    size_t pidx = pfP_get(S, x, y);
    pf_Point *p = &S->pts.list[pidx];
    int valid = dist < p->dist;
    if (valid) {
        p->dist = dist, p->esti = esti, p->prev = S->pts.curr;
        pfH_push(S, pidx);
    }
    lua_pushboolean(L, valid);
    return 1;
}

static size_t pf_neighbors(pf_State* S, pf_Point* p) {
    lua_State* L = S->L;
    size_t len = S->pts.size;
    lua_pushvalue(L, 6);
    lua_pushcfunction(L, Lpf_addpt);
    lua_pushinteger(L, p->x);
    lua_pushinteger(L, p->y);
    lua_pushnumber(L, p->dist);
    lua_pushnumber(L, p->esti);
    lua_call(L, 5, 0);
    return len;
}

static int pf_pushresult(pf_State* S, int x, int y, size_t ridx) {
    pf_Point* r = &S->pts.list[ridx];
    lua_State* L = S->L;
    int len = 0;
    lua_newtable(L);
    while (1) {
        pf_Point* prev = &S->pts.list[r->prev];
        //lua_createtable(L, 0, 2);
        //lua_pushinteger(L, r->x); lua_setfield(L, -2, "x");
        //lua_pushinteger(L, r->y); lua_setfield(L, -2, "y");
        //lua_rawseti(L, -2, ++len);
        lua_pushinteger(L, r->x); lua_rawseti(L, -2, ++len);
        lua_pushinteger(L, r->y); lua_rawseti(L, -2, ++len);
        if (r->x == x && r->y == y) break;
        r = prev;
    }
    return 1;
}

static int Lpathfinding(lua_State* L) {
    pf_State* S = pf_defaultstate(L);
    int sx = (int)luaL_checkinteger(L, 1);
    int sy = (int)luaL_checkinteger(L, 2);
    float esti = (float)luaL_optnumber(L, 3, INFINITY);
    int tx = (int)luaL_checkinteger(L, 4);
    int ty = (int)luaL_checkinteger(L, 5);
    int strict = (int)lua_toboolean(L, 7);
    size_t start, nearest;
    luaL_checktype(L, 6, LUA_TFUNCTION);
    S->heap.maxsize = (size_t)luaL_optinteger(L, 8, PF_MAX_HEAP);
    S->pts.size = S->heap.size = 0;
    pfT_clear(S);
    pfH_push(S, nearest = start = pfP_get(S, sx, sy));
    S->pts.list[0].dist = 0.f, S->pts.list[0].esti = esti;
    while (S->heap.size > 0) {
        size_t pidx = pfH_pop(S);
        pf_Point *p = &S->pts.list[pidx];
        if (p->esti < S->pts.list[nearest].esti)
            nearest = p - S->pts.list;
        if (p->x == tx && p->y == ty)
            return pf_pushresult(S, sx, sy, pidx);
        S->pts.curr = p - S->pts.list;
        pf_neighbors(S, p);
    }
    return !strict ? pf_pushresult(S, sx, sy, nearest) : 0;
}

LUALIB_API int luaopen_pathfinding(lua_State* L) {
    lua_pushcfunction(L, Lpathfinding);
    return 1;
}

/* maccc: flags+='-undefined dynamic_lookup'
 * unixcc: flags+='-coverage -ggdb -fPIC -shared' output='pathfinding.so'
 * win32cc: flags+="-mdll -DLUA_BUILD_AS_DLL" libs+='-llua54' output='pathfinding.dll' */