Created
January 19, 2022 07:34
-
-
Save starwing/2522ae70bf109137db71f1e3282ee313 to your computer and use it in GitHub Desktop.
A* Path finding in Lua
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/* | |
* 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' */ |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment