scout.c

   1 #include <stdlib.h>
   2 #include "scout.h"
   3
   4 scnode *scnodalloc(void *data)
   5 {
   6         scnode *nod = malloc(sizeof(scnode));
   7         nod->way = NULL;
   8         nod->dat = data;
   9         return nod;
  10 }
  11
  12 scway *scaddway(scnode *from, const scnode *to, float len, void *data)
  13 {
  14         scway *way = malloc(sizeof(scway));
  15         way->lto = to;
  16         way->alt = NULL;
  17         way->len = len;
  18         way->dat = data;
  19         scway *par = __scnodgetway(from);
  20         if (par)
  21                 par->alt = way;
  22         else
  23                 from->way = way;
  24         return way;
  25 }
  26
  27 int scisconnected(scnode *n1, scnode *n2) {
  28         for (scway *way = n1->way; way != NULL; way = way->alt) {
  29                 if (way->lto == n2)
  30                         return 1;
  31         }
  32         return 0;
  33 }
  34
  35 scwaypoint *scout(const scnode *from, const scnode *to, scwaypoint *stack)
  36 {
  37         scwaypoint *wayp = NULL;
  38         if (from == to)
  39                 return __scallocwayp(from, NULL);
  40         scwaypoint *stackend = __scstackgetend(stack);
  41         for (scway *way = from->way; way != NULL; way = way->alt) {
  42                 if (__scstackfind(stack, way))
  43                         continue;
  44                 scwaypoint *twayp = __scallocwayp(from, way);
  45                 if (stack)
  46                         stackend->nxt = twayp;
  47                 if ((twayp->nxt = scout(way->lto, to, stack ? stack : twayp)))
  48                         twayp->len += twayp->nxt->len;
  49                 if (twayp->nxt && (! wayp || wayp->len > twayp->len)) {
  50                         scdestroypath(wayp);
  51                         wayp = twayp;
  52                 } else {
  53                         scdestroypath(twayp);
  54                 }
  55         }
  56         return stack ? (stackend->nxt = wayp) : wayp;
  57 }
  58
  59 void scdestroypath(scwaypoint *stack)
  60 {
  61         for (scwaypoint *sptr = stack; sptr != NULL; sptr = sptr->nxt)
  62                 free(sptr);
  63 }
  64
  65 scway *__scnodgetway(const scnode *node)
  66 {
  67         scway *way;
  68         for (way = node->way; way != NULL && way->alt != NULL; way = way->alt);
  69         return way;
  70 }
  71
  72 scwaypoint *__scallocwayp(const scnode *node, const scway *way)
  73 {
  74         scwaypoint *wayp = malloc(sizeof(scwaypoint));
  75         wayp->nod = node;
  76         wayp->way = way;
  77         wayp->nxt = NULL;
  78         wayp->len = way ? way->len : 0.0f;
  79         return wayp;
  80 }
  81
  82 int __scstackfind(const scwaypoint *stack, const scway *way)
  83 {
  84         for (const scwaypoint *sptr = stack; sptr != NULL; sptr = sptr->nxt)
  85                 if (sptr->nod == way->lto)
  86                         return 1;
  87         return 0;
  88 }
  89
  90 scwaypoint *__scstackgetend(scwaypoint *stack)
  91 {
  92         scwaypoint *sptr;
  93         for (sptr = stack; sptr != NULL && sptr->nxt != NULL; sptr = sptr->nxt);
  94         return sptr;
  95 }