ref: 71250575dc797b28a5e9a9e8fd612c1253be9bc1
parent: c9f95c308362fed2f2d9cb5a309b1ed0338594e6
author: qwx <qwx@sciops.net>
date: Wed Mar 23 18:07:11 EDT 2022
implement dijkstra search
--- a/path/dat.h
+++ b/path/dat.h
@@ -11,6 +11,7 @@
int closed;
double g;
double h;
+ double Δg;
Node *to;
Node *from;
Pairheap *pq;
--- a/path/dijkstra.c
+++ b/path/dijkstra.c
@@ -10,19 +10,99 @@
Node *start, *goal;
-static int
-dijkstra(Node *a, Node *g)
+static void
+backtrack(void)
{- USED(a,g);
- return 0;
+ Node *n;
+
+ assert(goal != start);
+ for(n=goal; n!=start; n=n->from)
+ n->from->to = n;
}
+static Node **
+successors(Node *n)
+{+ static Node *suc[8+1];
+ static dtab[2*(nelem(suc)-1)]={+ -1,-1, 0,-1, 1,-1,
+ -1,0, 1,0,
+ -1,1, 0,1, 1,1,
+ };
+ int i;
+ Node *s, **np;
+ Point p;
+ Rectangle r;
+
+ memset(suc, 0, sizeof suc);
+ p = n2p(n);
+ r = Rect(0, 0, mapwidth, mapheight);
+ for(i=0, np=suc; i<nelem(dtab); i+=2){+ if(!ptinrect(addpt(p, Pt(dtab[i], dtab[i+1])), r))
+ continue;
+ s = n + dtab[i+1] * mapwidth + dtab[i];
+ assert(s >= map && s < map + mapwidth * mapheight);
+ if(isblocked(s))
+ continue;
+ s->Δg = dtab[i] != 0 && dtab[i+1] != 0 ? SQRT2 : 1;
+ *np++ = s;
+ }
+ return suc;
+}
+
+static Node *
+dijkstra(Node *a, Node *b)
+{+ double g, Δg;
+ Node *x, *s, **sl;
+ Pairheap *queue, *pn;
+
+ assert(a != nil && b != nil);
+ assert(a != b);
+ queue = nil;
+ x = a;
+ a->pq = pushqueue(0, a, &queue);
+ while((pn = popqueue(&queue)) != nil){+ x = pn->aux;
+ free(pn);
+ if(x == b)
+ break;
+ x->closed = 1;
+ if((sl = successors(x)) == nil)
+ sysfatal("a∗: %r");+ for(s=*sl++; s!=nil; s=*sl++){+ if(s->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;
+ 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);
+ }
+ }
+ }
+ nukequeue(&queue);
+ return x;
+}
+
static int
findpath(void)
{if(start == nil || goal == nil)
return -1;
- return dijkstra(start, goal);
+ resetmap();
+ if(dijkstra(start, goal) != goal)
+ return -1;
+ backtrack();
+ return 0;
}
int
@@ -29,8 +109,8 @@
mouseinput(Node *n, Mouse m)
{ switch(m.buttons & 7){- case 1: goal = n; findpath(); break;
- case 2: start = n; findpath(); break;
+ case 1: if(start != n) goal = n; return findpath();
+ case 2: if(goal != n) start = n; return findpath();
case 4: n->blocked ^= 1; break;
}
return 0;