shithub: asif

--- a/path/a∗.c

+++ b/path/a∗.c

@@ -8,6 +8,21 @@

 #include "dat.h"

 #include "fns.h"

+/* FIXME: nope, non-optimal paths + too many nodes opened, again

+ * (dijsktra works fine) */

+/* 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; does

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

+static double

+movecost(int Δx, int Δy)

+{

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

+}

 static Node *

 a∗(Node *a, Node *b)

@@ -18,8 +33,8 @@

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

 	assert(a != b);

 	queue = nil;

-	x = a;

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

+	x = a;

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

 		x = pn->aux;

 		free(pn);

@@ -26,6 +41,8 @@

 		if(x == b)

 			break;

 		x->closed = 1;

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

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

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

 			sysfatal("a∗: %r");

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

@@ -39,8 +56,12 @@

 				s->open = 1;

 				s->h = distfn(s, b);

 				s->g = g;

+				dprint(Logtrace, "a∗: 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->h, s, &queue);

 			}else if(Δg > 0){

+				dprint(Logtrace, "a∗: decrease [%#p,%P] h %.4f g %.4f Δg %.4f → f %.4f\n",

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

 				s->from = x;

 				s->g -= Δg;

 				decreasekey(s->pq, Δg, &queue);

@@ -56,6 +77,6 @@

 threadmain(int argc, char **argv)

 	init(argc, argv);

-	initgrid(a∗);

+	initgrid(a∗, movecost);

 	evloop();

--- a/path/bfs.c

+++ b/path/bfs.c

@@ -42,6 +42,6 @@

 threadmain(int argc, char **argv)

 	init(argc, argv);

-	initgrid(bfs);

+	initgrid(bfs, nil);

 	evloop();

--- a/path/client.c

+++ b/path/client.c

@@ -9,6 +9,8 @@

 #include "dat.h"

 #include "fns.h"

+mainstacksize = 512*1024;

 extern QLock drawlock;

 Node*	scrselect(Point);

 void	updatedrw(void);

@@ -86,7 +88,7 @@

 	mapheight = 64;

 	ARGBEGIN{

 	case 'D':

-		if(++debuglevel >= Logtrace)

+		if(++debuglevel >= Logparanoid)

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

 		break;

 	case '4':

@@ -106,6 +108,9 @@

 		break;

 	default: usage();

 	}ARGEND

+	if(mapwidth <= 0 || mapwidth > 512

+	|| mapheight <= 0 || mapheight > 512)

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

 	fmtinstall('P', Pfmt);

 	fmtinstall('R', Rfmt);

 	initfs();

--- a/path/dat.h

+++ b/path/dat.h

@@ -31,4 +31,3 @@

 extern double	(*distfn)(Node*, Node*);

 extern Node**	(*successorfn)(Node*);

-extern Node*	(*pathfn)(Node*, Node*);

--- a/path/dijkstra.c

+++ b/path/dijkstra.c

@@ -8,6 +8,15 @@

 #include "dat.h"

 #include "fns.h"

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

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

+static double

+movecost(int Δx, int Δy)

+{

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

+}

 static Node *

 dijkstra(Node *a, Node *b)

@@ -18,8 +27,8 @@

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

 	assert(a != b);

 	queue = nil;

-	x = a;

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

+	x = a;

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

 		x = pn->aux;

 		free(pn);

@@ -26,6 +35,8 @@

 		if(x == 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++){

@@ -38,12 +49,9 @@

 				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);

-			}else if(Δg > 0){

-				s->from = x;

-				s->g -= Δg;

-				decreasekey(s->pq, Δg, &queue);

-				assert(s->g >= 0);

@@ -55,6 +63,6 @@

 threadmain(int argc, char **argv)

 	init(argc, argv);

-	initgrid(dijkstra);

+	initgrid(dijkstra, movecost);

 	evloop();

--- a/path/fns.h

+++ b/path/fns.h

@@ -1,7 +1,7 @@

 void	init(int, char**);

 void	evloop(void);

 void	initfs(void);

-void	initgrid(Node* (*)(Node*, Node*));

+void	initgrid(Node* (*)(Node*, Node*), double (*)(int, int));

 void	resetmap(void);

 void	clearmap(void);

 void	initmap(void);

--- a/path/grid.c

+++ b/path/grid.c

@@ -13,10 +13,12 @@

 double	(*distfn)(Node*, Node*);

 Node**	(*successorfn)(Node*);

-Node*	(*pathfn)(Node*, Node*);

 Node *start, *goal;

+static Node*	(*pathfn)(Node*, Node*);

+static double	(*costfn)(int, int);

 static void

 backtrack(void)

@@ -32,8 +34,8 @@

 	static Node *suc[8+1];

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

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

-		1,-1, 1,1, -1,1, -1,-1,

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

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

};

 	int i;

 	Node *s, **np;

@@ -50,7 +52,7 @@

 		assert(s >= map && s < map + mapwidth * mapheight);

 		if(isblocked(s))

 			continue;

-		s->Δg = dtab[i] != 0 && dtab[i+1] != 0 ? 1+0.001 : 1.0;

+		s->Δg = costfn != nil ? costfn(dtab[i], dtab[i+1]) : 0;

 		*np++ = s;

 	return suc;

@@ -60,10 +62,12 @@

 successors4(Node *n)

 	static Node *suc[4+1];

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

-		0,-1, -1,0, 1,0, 0,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;

+	int i, *t;

 	Node *s, **np;

 	Point p;

 	Rectangle r;

@@ -71,14 +75,17 @@

 	memset(suc, 0, sizeof suc);

 	p = n2p(n);

 	r = Rect(0, 0, mapwidth, mapheight);

+	/* path straightening; cf.:

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

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

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

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

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

 			continue;

-		s = n + dtab[i+1] * mapwidth + dtab[i];

+		s = n + t[i+1] * mapwidth + t[i];

 		assert(s >= map && s < map + mapwidth * mapheight);

 		if(isblocked(s))

 			continue;

-		s->Δg = 1;

+		s->Δg = costfn != nil ? costfn(t[i], t[i+1]) : 0;

 		*np++ = s;

 	return suc;

@@ -92,6 +99,8 @@

 		return -1;

 	resetmap();

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

+		start, n2p(start), goal, n2p(goal));

 	if(pathfn(start, goal) != goal){

 		werrstr("findpath: failed");

 		return -1;

@@ -120,7 +129,7 @@

 void

-initgrid(Node* (*f)(Node*, Node*))

+initgrid(Node* (*f)(Node*, Node*), double (*cost)(int, int))

 	if(fourdir){

 		distfn = manhdist;

@@ -132,4 +141,5 @@

 	mousefn = grmouse;

 	keyfn = grkey;

 	pathfn = f;

+	costfn = cost;

--- a/pheap.c

+++ b/pheap.c

@@ -6,6 +6,11 @@

  * heap: A new form of self-adjusting heap.  Algorithmica 1, 111–129

  * (1986).  */

+/* this implementation requires a bigger stack size if the heap can

+ * grow big (8192 is already insufficient with 40-50 nodes);

+ * otherwise, stack overflows hidden behind more cryptic memory pool

+ * corruption errors will occur */

 static void

 checkheap(Pairheap *p, Pairheap *queue)

@@ -53,7 +58,7 @@

 static void

 debugqueue(Pairheap **queue)

-	if(debuglevel < Logtrace || queue == nil)

+	if(debuglevel < Logparanoid || queue == nil)

 		return;

 	printqueue(queue);

 	checkheap(*queue, *queue);

@@ -62,7 +67,7 @@

 static Pairheap *

 mergequeue(Pairheap *a, Pairheap *b)

-	dprint(Logtrace, "pheap::mergequeue %#p %.6f b %#p %.6f\n",

+	dprint(Logparanoid, "pheap::mergequeue %#p %.6f b %#p %.6f\n",

 		a, a->n, b, b!=nil ? b->n : NaN());

 	if(b == nil)

 		return a;

@@ -108,7 +113,7 @@

 	Pairheap *p;

-	dprint(Logtrace, "pheap::nukequeue %#p\n", queue);

+	dprint(Logparanoid, "pheap::nukequeue %#p\n", queue);

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

 		debugqueue(&p);

 		free(p);

@@ -123,7 +128,7 @@

 	p = *queue;

 	if(p == nil)

 		return nil;

-	dprint(Logtrace, "pheap::popqueue %#p %.6f\n", p, p->n);

+	dprint(Logparanoid, "pheap::popqueue %#p %.6f\n", p, p->n);

 	*queue = mergepairs(p->left, p);

 	debugqueue(queue);

 	return p;

@@ -134,7 +139,7 @@

 	Pairheap *q;

-	dprint(Logtrace, "pheap::decreasekey %#p %.6f Δ %.6f\n", p, p->n, Δ);

+	dprint(Logparanoid, "pheap::decreasekey %#p %.6f Δ %.6f\n", p, p->n, Δ);

 	p->n -= Δ;

 	if(p->parent != nil && p->n < p->parent->n){

 		assert(p->parent->left != nil);

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

 	p = emalloc(sizeof *p);

 	p->n = n;

 	p->aux = aux;

-	dprint(Logtrace, "pheap::pushqueue %#p %.6f\n", p, p->n);

+	dprint(Logparanoid, "pheap::pushqueue %#p %.6f\n", p, p->n);

 	*queue = mergequeue(p, *queue);

 	debugqueue(queue);

 	return p;