/* return -1 on no path */
int dijkstra(int start,int goal) {
static int dist[MAXV]; /* array of distances */
static char vis[MAXV]; /* array indicating visited (processed) nodes */
int val,w,cur,i,x2;
if(start==goal) return 0;
memset(vis,0,n);
memset(dist,126,n*sizeof(int));
dist[start]=0; /* set distance to start to 0 */
heapn=1;
heapinsert(0,start); /* insert start node to heap */
while(heapn>1) {
heapremove(&val,&cur);
if(vis[cur]) continue; /* if current node is already done: continue */
if(cur==goal) return val; /* done! */
vis[cur]=1;
for(i=gs[cur];i<gs[cur+1];i++) {
x2=to[i];
if(vis[x2]) continue;
w=val+cost[i];
if(dist[x2]>w) {
dist[x2]=w;
heapinsert(w,x2);
}
}
}
return -1;
}
/* dijkstra template */
int dijkstra_template(int start,int goal) {
static int dist[]; /* array of distances */
static char vis[]; /* array indicating visited (processed) nodes */
int val,w,cur;
if(start==goal) return 0;
/* set visited to 0 */
/* set distances to INF */
dist[start]=0; /* set distance to start to 0 */
heapn=1;
heapinsert(0,start); /* insert start node to heap */
while(heapn>1) {
heapremove(&val,&cur);
if(vis[cur]) continue; /* if current node is already done: continue */
if(cur==goal) return val; /* done! */
vis[cur]=1;
for(neighbours to cur) {
x2=potential neighbour;
if(x2 is out of bounds) continue;
if(vis[x2]) continue;
w=val+calcdist(cur,x2);
if(dist[x2]>w) {
dist[x2]=w;
heapinsert(w,x2);
}
}
}
/* node id: bit 0-6:from, bit 7-13: to, bit 13: side from, bit 14: side to */
double dijkstra() {
static double D[MAXV]; /* array of distances */
static char vis[MAXV]; /* array indicating visited (processed) nodes */
double val,w;
int cur,i,x2;
int start=0;
int curfrom,curto,next;
int sidefrom,sideto,side;
memset(vis,0,MAXV);
for(i=0;i<MAXV;i++) D[i]=INF;
D[start]=0; /* set distance to start to 0 */
heapn=1;
heapinsert(0,start); /* insert start node to heap */
while(heapn>1) {
heapremove(&val,&cur);
if(vis[cur]) continue; /* if current node is already done: continue */
curfrom=cur&127; curto=(cur>>7)&127;
sidefrom=(cur>>14)&1; sideto=cur>>15;
// printf("at %d %d %d %d: %f %f\n",sidefrom,sideto,curfrom,curto,D[cur],val);
if(curto==1) return val; /* done! */
vis[cur]=1;
for(side=0;side<2;side++) for(next=1;next<n;next++) if(dist[sideto][side][curto][next]>-.5) {
if(side==1 && next==1) continue;
x2=curto+(next<<7)+(sideto<<14)+(side<<15);
if(vis[x2]) continue;
w=val+dist[sideto][side][curto][next];
w+=arclength(circler[curto],theta2[sidefrom][sideto][curfrom][curto],
theta1[sideto][side][curto][next]);
// printf("go to %d %d %d %d, cur %f + arc %f + line %f = %f\n",sideto,side,curto,next,val,arclength(circler[curto],theta2[sidefrom][sideto][curfrom][curto],theta1[sideto][side][curto][next]),dist[sideto][side][curto][next],w);
if(D[x2]>w) {
D[x2]=w;
if(heapn==MAXH) puts("ERROR");
heapinsert(w,x2);
}
}
}
puts("error, no path");
return 0;
}
/*** Makes a heap of all table entries ***/
static int table2heap(table_t *t)
{
int i;
/*** Fill result heap ***/
for (i=0; i<TABLESIZE; i++) {
entry_t *p = t->table[i];
while (p) {
heapinsert(t, p);
p = p->next;
}
}
return 1;
}
