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