shithub: asif

Info  •  Files  •  Log

fix up dijkstra search code and add to path

department of redundancy department, but at least it works

--- a/app/path/drw.c

+++ b/app/path/drw.c

@@ -166,7 +166,7 @@

 	viewr = Rpt(ZP, Pt(gridwidth*Nodesz+1, gridheight*Nodesz+1));

 	viewΔ = divpt(addpt(subpt(ZP, subpt(screen->r.max, screen->r.min)), viewr.max), 2);

 	hudr.min = addpt(screen->r.min, subpt(Pt(2, viewr.max.y+2), viewΔ));

-	hudr.max = addpt(hudr.min, Pt(viewr.max.x-2, font->height*3));

+	hudr.max = addpt(hudr.min, Pt(screen->r.max.x, font->height*3));

 	freeimage(view);

 	view = eallocimage(viewr, 0, DNofill);

 	draw(screen, screen->r, col[Cbg], nil, ZP);

--- a/app/path/map.c

+++ b/app/path/map.c

@@ -30,7 +30,7 @@

 	switch(alg){

 	case Pa∗: pathfn = a∗findpath; break;

 	case Pbfs: pathfn = bfsfindpath; break;

-	//case Pdijkstra: pathfn = dijkstrafindpath; break;

+	case Pdijkstra: pathfn = dijkstrafindpath; break;

 	default: sysfatal("setparm: unknown algo type %d", alg);

 	}

 	switch(dist){

--- a/app/path/mkfile

+++ b/app/path/mkfile

@@ -2,10 +2,10 @@

 BIN=$home/bin/$objtype/asif

 TARG=path

 HFILES=../../asif.h ../../path/path.h dat.h fns.h

-#	../../path/dijkstra.$O\

 OFILES=\

 	../../path/a∗.$O\

 	../../path/bfs.$O\

+	../../path/dijkstra.$O\

 	../../path/grid.$O\

 	../../dprint.$O\

 	../../emalloc.$O\

--- a/app/path/path.c

+++ b/app/path/path.c

@@ -66,7 +66,7 @@

 static void

 usage(void)

 {

-	fprint(2, "usage: %s [-D4] [-s width[,height]]\n", argv0);

+	fprint(2, "usage: %s [-D4] [-a algo] [-d dist] [-s width[,height]]\n", argv0);

 	threadexits("usage");

 }

@@ -73,13 +73,14 @@

 void

 threadmain(int argc, char **argv)

 {

-	int w, h, d, m;

+	int w, h, a, d, m;

 	char *s;

 	w = 64;

 	h = 64;

-	d = Move8;

-	m = Doctile;

+	a = -1;

+	d = -1;

+	m = Move8;

 	ARGBEGIN{

 	case 'D':

 		if(++debuglevel >= Logparanoid)

@@ -86,9 +87,35 @@

 			mainmem->flags |= POOL_NOREUSE | POOL_PARANOIA | POOL_LOGGING;

 		break;

 	case '4':

-		d = Move4;

-		m = Dmanhattan;

+		m = Move4;

+		d = Dmanhattan;

 		break;

+	case 'a':

+		s = EARGF(usage());

+		if(strcmp(s, "a∗") == 0)

+			a = Pa∗;

+		else if(strcmp(s, "dijkstra") == 0)

+			a = Pdijkstra;

+		else if(strcmp(s, "bfs") == 0)

+			a = Pbfs;

+		else{

+			fprint(2, "unsupported algorithm\n");

+			usage();

+		}

+		break;

+	case 'd':

+		s = EARGF(usage());

+		if(strcmp(s, "octile") == 0)

+			d = Doctile;

+		else if(strcmp(s, "manhattan") == 0)

+			d = Dmanhattan;

+		else if(strcmp(s, "euclid") == 0)

+			d = Deuclid;

+		else{

+			fprint(2, "unsupported distance function\n");

+			usage();

+		}

+		break;

 	case 's':

 		w = strtol(EARGF(usage()), &s, 0);

 		if(w <= 0)

@@ -108,7 +135,11 @@

 		sysfatal("invalid map size, must be in ]0,512]");

 	keyfn = grkey;

 	mousefn = grmouse;

-	setparm(d, Pa∗, m);

+	if(d < 0)

+		d = m == Move8 ? Doctile : Dmanhattan;

+	if(a < 0)

+		a = Pa∗;

+	setparm(m, a, d);

 	init(w, h);

 	evloop();

 }

--- a/path/a∗.c

+++ b/path/a∗.c

@@ -4,8 +4,6 @@

 #include "asif.h"

 #include "path.h"

-/* FIXME: assumes grid */

-

 typedef Vertex Point;

 typedef struct PNode PNode;

@@ -44,9 +42,7 @@

  * https://www.redbloblgames.com/pathfinding/a-star/implementation.html

  * one addition: make cost function to increase at a slower rate to

  * resolve tie-breakers in favor of closer nodes, otherwise we will

- * explore all nodes in the rectangle between the two points; does

- * NOT work with 4-way movement and manhattan distance for some reason */

- 	// FIXME: ^- reverify that it does not work

+ * explore all nodes in the rectangle between the two points */

 static double

 movecost(int Δx, int Δy)

 {

--- a/path/bfs.c

+++ b/path/bfs.c

@@ -13,8 +13,12 @@

 static PNode**	(*successorfn)(Node*);

 static void

-cleanup(void)

+backtrack(Node *a, Node *b)

 {

+	Node *n;

+

+	for(n=b; n!=a; n=n->from)

+		n->from->to = n;

 }

 static double

@@ -110,6 +114,9 @@

 		}

 	}

 	vecfree(front);

+	if(u != b)

+		return -1;

+	backtrack(a, b);

 	return 0;

 }

--- a/path/dijkstra.c

+++ b/path/dijkstra.c

@@ -4,65 +4,184 @@

 #include <draw.h>

 #include <mouse.h>

 #include <keyboard.h>

-#include "../asif.h"

-#include "dat.h"

-#include "fns.h"

+#include "asif.h"

+#include "path.h"

 /* currently uniform cost search and no reprioritizing (decrease-key operation) */

-/* explore in all directions at the same time */

+typedef Vertex Point;

+

+typedef struct PNode PNode;

+struct PNode{

+	Node *n;

+	double g;

+	double Δg;

+	Pairheap *pq;

+};

+

+static PNode**	(*successorfn)(Node*);

+static Zpool *zpool;

+

+static void

+cleanup(Pairheap **queue)

+{

+	Pairheap *p;

+

+	while((p = popqueue(queue)) != nil){

+		zfree(p->aux);

+		free(p);

+	}

+}

+

+static void

+backtrack(Node *a, Node *b)

+{

+	Node *n;

+

+	for(n=b; n!=a; n=n->from)

+		n->from->to = n;

+}

+

+/* slightly penalize diagonal movement for nicer-looking paths; cf.:

+ * https://www.redbloblgames.com/pathfinding/a-star/implementation.html

+ * one addition: make cost function to increase at a slower rate to

+ * resolve tie-breakers in favor of closer nodes, otherwise we will

+ * explore all nodes in the rectangle between the two points */

 static double

 movecost(int Δx, int Δy)

 {

-	return Δx != 0 && Δy != 0 ? SQRT2 : 1.0;

+	return Δx != 0 && Δy != 0 ? 1.001 : 1.0;

 }

-static Node *

+static PNode **

+successors8(Node *nu)

+{

+	static PNode *suc[8+1];

+	static dtab[2*(nelem(suc)-1)]={

+		1,0, 0,-1, -1,0, 0,1,

+		-1,-1, -1,1, 1,-1, 1,1,

+	};

+	int i;

+	Node *nv;

+	PNode *v, **vp;

+	Point p;

+	Rectangle r;

+

+	memset(suc, 0, sizeof suc);

+	p = n2p(nu);

+	r = Rect(0, 0, gridwidth, gridheight);

+	for(i=0, vp=suc; i<nelem(dtab); i+=2){

+		if(!ptinrect(addpt(p, Pt(dtab[i], dtab[i+1])), r))

+			continue;

+		nv = nu + dtab[i+1] * gridwidth + dtab[i];

+		assert(nv >= grid && nv < grid + gridwidth * gridheight);

+		if(isblocked(nv))

+			continue;

+		v = zalloc(zpool);

+		v->n = nv;

+		v->Δg = movecost(dtab[i], dtab[i+1]);

+		*vp++ = v;

+	}

+	return suc;

+}

+

+static PNode **

+successors4(Node *nu)

+{

+	static PNode *suc[4+1];

+	static int dtab[2*(nelem(suc)-1)]={

+		1,0, -1,0, 0,-1, 0,1,

+	}, rdtab[nelem(dtab)]={

+		0,1, 0,-1, -1,0, 1,0,

+	};

+	int i, *t;

+	Node *nv;

+	PNode *v, **vp;

+	Point p;

+	Rectangle r;

+

+	memset(suc, 0, sizeof suc);

+	p = n2p(nu);

+	r = Rect(0, 0, gridwidth, gridheight);

+	/* path straightening; cf.:

+	 * https://www.redbloblgames.com/pathfinding/a-star/implementation.html */

+	t = (p.x + p.y) % 2 == 0 ? rdtab : dtab;

+	for(i=0, vp=suc; i<nelem(dtab); i+=2){

+		if(!ptinrect(addpt(p, Pt(t[i], t[i+1])), r))

+			continue;

+		nv = nu + t[i+1] * gridwidth + t[i];

+		assert(nv >= grid && nv < grid + gridwidth * gridheight);

+		if(isblocked(nv))

+			continue;

+		v = zalloc(zpool);

+		v->n = nv;

+		v->Δg = movecost(t[i], t[i+1]);

+		*vp++ = v;

+	}

+	return suc;

+}

+

+static int

 dijkstra(Node *a, Node *b)

 {

 	double g, Δg;

-	Node *x, *s, **sl;

+	PNode *u, *v, **vl;

+	Node *nu, *nv;

 	Pairheap *queue, *pn;

-	assert(a != nil && b != nil);

-	assert(a != b);

 	queue = nil;

-	a->pq = pushqueue(0, a, &queue);

-	x = a;

+	nu = a;

+	u = zalloc(zpool);

+	u->n = a;

+	u->pq = pushqueue(distfn(a, b), u, &queue);

 	while((pn = popqueue(&queue)) != nil){

-		x = pn->aux;

+		u = pn->aux;

+		nu = u->n;

 		free(pn);

-		if(x == b)

+		if(nu == b)

 			break;

-		x->closed = 1;

-		dprint(Logtrace, "dijkstrdijkstra: closed [%#p,%P] h %.4f g %.4f\n",

-			x, n2p(x), x->h, x->g);

-		if((sl = successorfn(x)) == nil)

-			sysfatal("dijkstra: %r");

-		for(s=*sl++; s!=nil; s=*sl++){

-			if(s->closed)

+		nu->closed = 1;

+		dprint(Logtrace, "dijkstra: closed [%#p,%P] g %.4f\n",

+			u, n2p(nu), u->g);

+		if((vl = successorfn(nu)) == nil)

+			sysfatal("a∗: %r");

+		for(v=*vl++; v!=nil; v=*vl++){

+			nv = v->n;

+			if(nv->closed)

 				continue;

-			assert(!isblocked(s));

-			g = x->g + s->Δg;

-			Δg = s->g - g;

-			if(!s->open){

-				s->from = x;

-				s->open = 1;

-				s->g = g;

-				dprint(Logtrace, "dijkstra: opened [%#p,%P] h %.4f g %.4f f %.4f\n",

-					s, n2p(s), s->h, s->g, s->h + s->g);

-				s->pq = pushqueue(s->g, s, &queue);

+			g = u->g + v->Δg;

+			Δg = v->g - g;

+			if(!nv->open){

+				nv->from = nu;

+				nv->open = 1;

+				v->g = g;

+				dprint(Logtrace, "dijkstra: opened [%#p,%P] g %.4f\n",

+					v, n2p(nv), v->g);

+				v->pq = pushqueue(v->g, v, &queue);

 			}

+			assert(Δg <= 0);

 		}

 	}

-	nukequeue(&queue);

-	return x;

+	cleanup(&queue);

+	if(nu != b)

+		return -1;

+	backtrack(a, b);

+	return 0;

 }

-void

-threadmain(int argc, char **argv)

+int

+dijkstrafindpath(Node *a, Node *b)

 {

-	init(argc, argv);

-	initgrid(dijkstra, movecost);

-	evloop();

+	assert(a != nil && b != nil && a != b);

+	clearpath();

+	if(zpool == nil)

+		zpool = znew(sizeof(PNode));

+	successorfn = movemode == Move8 ? successors8 : successors4;

+	dprint(Logdebug, "grid::dijkstrafindpath: dijkstra from [%#p,%P] to [%#p,%P]\n",

+		a, n2p(a), b, n2p(b));

+	if(dijkstra(a, b) < 0){

+		dprint(Logdebug, "grid::dijkstrafindpath: failed to find a path\n");

+		return -1;

+	}

+	return 0;

 }