source: subversion/applications/rendering/gosmore/libgosm.cpp @ 10694

Last change on this file since 10694 was 10507, checked in by nic, 11 years ago

Smaller .pak files. Render map with less clutter. Less gcc warings.

  • Property svn:executable set to *
File size: 25.0 KB
Line 
1#include "libgosm.h"
2
3routeNodeType *route = NULL, *shortest = NULL, **routeHeap;
4int dhashSize, routeHeapSize, tlat, tlon, flat, flon, rlat, rlon;
5int *hashTable, bucketsMin1, pakHead = 0xEB3A942;
6char *gosmData, *gosmSstr[searchCnt];
7ndType *ndBase;
8styleStruct *style;
9wayType *gosmSway[searchCnt];
10
11int TagCmp (char *a, char *b)
12{ // This works like the ordering of books in a library : We ignore
13  // meaningless words like "the", "street" and "north". We (should) also map
14  // deprecated words to their new words, like petrol to fuel
15  // TODO : We should consider an algorithm like double metasound.
16  static const char *omit[] = { /* "the", in the middle of a name ?? */
17    "ave", "avenue", "blvd", "boulevard", "byp", "bypass",
18    "cir", "circle", "close", "cres", "crescent", "ct", "court", "ctr",
19      "center",
20    "dr", "drive", "hwy", "highway", "ln", "lane", "loop",
21    "pass", "pky", "parkway", "pl", "place", "plz", "plaza",
22    /* "run" */ "rd", "road", "sq", "square", "st", "street",
23    "ter", "terrace", "tpke", "turnpike", /*trce, trace, trl, trail */
24    "walk",  "way"
25  };
26  static const char *words[] = { "", "first", "second", "third", "fourth",
27    "fifth", "sixth", "seventh", "eighth", "nineth", "tenth", "eleventh",
28    "twelth", "thirteenth", "fourteenth", "fifthteenth", "sixteenth",
29    "seventeenth", "eighteenth", "nineteenth", "twentieth" };
30  static const char *teens[] = { "", "", "twenty ", "thirty ", "fourty ",
31    "fifty ", "sixty ", "seventy ", "eighty ", "ninety " };
32 
33  if (stricmp (a, "the ") == 0) a += 4;
34  if (stricmp (b, "the ") == 0) b += 4;
35  if (strchr ("WEST", a[0]) && a[1] == ' ') a += 2; // e.g. N 21st St
36  if (strchr ("WEST", b[0]) && b[1] == ' ') b += 2;
37
38  for (;;) {
39    char n[2][30] = { "", "" }, *ptr[2];
40    int wl[2];
41    for (int i = 0; i < 2; i++) {
42      char **p = i ? &b : &a;
43      if ((*p)[0] == ' ') {
44        for (int i = 0; i < int (sizeof (omit) / sizeof (omit[0])); i++) {
45          if (strncasecmp (*p + 1, omit[i], strlen (omit[i])) == 0 &&
46              !isalpha ((*p)[1 + strlen (omit[i])])) {
47            (*p) += 1 + strlen (omit[i]);
48            break;
49          }
50        }
51      }
52      if (isdigit (**p) && (!isdigit((*p)[1]) || !isdigit ((*p)[2]))
53              /* && isalpha (*p + strcspn (*p, "0123456789"))*/) {
54        // while (atoi (*p) > 99) (*p)++; // Buggy
55        if (atoi (*p) > 20) strcpy (n[i], teens[atoi ((*p)++) / 10]);
56        strcat (n[i], words[atoi (*p)]);
57        while (isdigit (**p) /*|| isalpha (**p)*/) (*p)++;
58        ptr[i] = n[i];
59        wl[i] = strlen (n[i]);
60      }
61      else {
62        ptr[i] = *p;
63        wl[i] = **p == ' ' ? 1 : strcspn (*p , " \n");
64      }
65    }
66    int result = strncasecmp (ptr[0], ptr[1], wl[0] < wl[1] ? wl[1] : wl[0]);
67    if (result || *ptr[0] == '\0' || *ptr[0] == '\n') return result;
68    if (n[0][0] == '\0') a += wl[1]; // In case b was 21st
69    if (n[1][0] == '\0') b += wl[0]; // In case a was 32nd
70  }
71}
72
73/* 1. Bsearch idx such that
74      ZEnc (way[idx]) < ZEnc (clon/lat) < ZEnc (way[idx+1])
75   2. Fill the list with ways around idx.
76   3. Now there's a circle with clon/clat as its centre and that runs through
77      the worst way just found. Let's say it's diameter is d. There exist
78      4 Z squares smaller that 2d by 2d that cover this circle. Find them
79      with binary search and search through them for the nearest ways.
80   The worst case is when the nearest nodes are far along a relatively
81   straight line.
82*/
83static int IdxSearch (int *idx, int h, char *key, unsigned z)
84{
85  for (int l = 0; l < h;) {
86    char *tag = gosmData + idx[(h + l) / 2];
87    int diff = TagCmp (tag, key);
88    while (*--tag) {}
89    if (diff > 0 || (diff == 0 &&
90      ZEnc ((unsigned)((wayType *)tag)[-1].clat >> 16, 
91            (unsigned)((wayType *)tag)[-1].clon >> 16) >= z)) h = (h + l) / 2;
92    else l = (h + l) / 2 + 1;
93  }
94  return h;
95}
96
97void GosmSearch (int clon, int clat, char *key)
98{
99  __int64 dista[searchCnt];
100  char *taga[searchCnt];
101  int *idx =
102    (int *)(ndBase + hashTable[bucketsMin1 + (bucketsMin1 >> 7) + 2]);
103  int l = IdxSearch (idx, hashTable - idx, key, 0), count;
104//  char *lastName = data + idx[min (hashTable - idx),
105//    int (sizeof (gosmSway) / sizeof (gosmSway[0]))) + l - 1];
106  int cz = ZEnc ((unsigned) clat >> 16, (unsigned) clon >> 16);
107  for (count = 0; count + l < hashTable - idx && count < searchCnt;) {
108    int m[2], c = count, ipos, dir, bits;
109    m[0] = IdxSearch (idx, hashTable - idx, gosmData + idx[count + l], cz);
110    m[1] = m[0] - 1;
111    __int64 distm[2] = { -1, -1 }, big = ((unsigned __int64) 1 << 63) - 1;
112    while (c < searchCnt && (distm[0] < big || distm[1] < big)) {
113      dir = distm[0] < distm[1] ? 0 : 1;
114      if (distm[dir] != -1) {
115        for (ipos = c; count < ipos && distm[dir] < dista[ipos - 1]; ipos--) {
116          dista[ipos] = dista[ipos - 1];
117          gosmSway[ipos] = gosmSway[ipos - 1];
118          taga[ipos] = taga[ipos - 1];
119        }
120        char *tag = gosmData + idx[m[dir]];
121        taga[ipos] = tag;
122        while (*--tag) {}
123        gosmSway[ipos] = (wayType*)tag - 1;
124        dista[ipos] = distm[dir];
125        c++;
126      }
127      m[dir] += dir ? 1 : -1;
128
129      if (0 <= m[dir] && m[dir] < hashTable - idx &&
130        TagCmp (gosmData + idx[m[dir]], gosmData + idx[count + l]) == 0) {
131        char *tag = gosmData + idx[m[dir]];
132        while (*--tag) {}
133        distm[dir] = Sqr ((__int64)(clon - ((wayType*)tag)[-1].clon)) +
134          Sqr ((__int64)(clat - ((wayType*)tag)[-1].clat));
135      }
136      else distm[dir] = big;
137    }
138    if (count == c) break; // Something's wrong. idx[count + l] not found !
139    if (c >= searchCnt) {
140      c = count; // Redo the adding
141      for (bits = 0; bits < 16 && dista[searchCnt - 1] >> (bits * 2 + 32);
142        bits++) {}
143/* Print Z's for first solution
144      for (int j = c; j < searchCnt; j++) {
145        for (int i = 0; i < 32; i++) printf ("%d%s",
146          (ZEnc ((unsigned) gosmSway[j]->clat >> 16,
147                 (unsigned) gosmSway[j]->clon >> 16) >> (31 - i)) & 1,
148          i == 31 ? " y\n" : i % 2 ? " " : "");
149      } */
150/* Print centre, up, down, right and left to see if they're in the square
151      for (int i = 0; i < 32; i++) printf ("%d%s", (cz >> (31 - i)) & 1,
152        i == 31 ? " x\n" : i % 2 ? " " : "");
153      for (int i = 0; i < 32; i++) printf ("%d%s", (
154        ZEnc ((unsigned)(clat + (int) sqrt (dista[searchCnt - 1])) >> 16,
155              (unsigned)clon >> 16) >> (31 - i)) & 1,
156        i == 31 ? " x\n" : i % 2 ? " " : "");
157      for (int i = 0; i < 32; i++) printf ("%d%s", (
158        ZEnc ((unsigned)(clat - (int) sqrt (dista[searchCnt - 1])) >> 16,
159              (unsigned)clon >> 16) >> (31 - i)) & 1,
160        i == 31 ? " x\n" : i % 2 ? " " : "");
161      for (int i = 0; i < 32; i++) printf ("%d%s", (
162        ZEnc ((unsigned)clat >> 16,
163              (unsigned)(clon + (int) sqrt (dista[searchCnt - 1])) >> 16) >> (31 - i)) & 1,
164        i == 31 ? " x\n" : i % 2 ? " " : "");
165      for (int i = 0; i < 32; i++) printf ("%d%s", (
166        ZEnc ((unsigned)clat >> 16,
167              (unsigned)(clon - (int) sqrt (dista[searchCnt - 1])) >> 16) >> (31 - i)) & 1,
168        i == 31 ? " x\n" : i % 2 ? " " : "");
169*/     
170      int swap = cz ^ ZEnc (
171        (unsigned) (clat + (clat & (1 << (bits + 16))) * 4 -
172                                              (2 << (bits + 16))) >> 16,
173        (unsigned) (clon + (clon & (1 << (bits + 16))) * 4 -
174                                              (2 << (bits + 16))) >> 16);
175      // Now we search through the 4 squares around (clat, clon)
176      for (int mask = 0, maskI = 0; maskI < 4; mask += 0x55555555, maskI++) {
177        int s = IdxSearch (idx, hashTable - idx, gosmData + idx[count + l],
178          (cz ^ (mask & swap)) & ~((4 << (bits << 1)) - 1));
179/* Print the square
180        for (int i = 0; i < 32; i++) printf ("%d%s",
181          (((cz ^ (mask & swap)) & ~((4 << (bits << 1)) - 1)) >> (31 - i)) & 1,
182          i == 31 ? "\n" : i % 2 ? " " : "");
183        for (int i = 0; i < 32; i++) printf ("%d%s",
184          (((cz ^ (mask & swap)) | ((4 << (bits << 1)) - 1)) >> (31 - i)) & 1,
185          i == 31 ? "\n" : i % 2 ? " " : "");
186*/
187        for (;;) {
188          char *tag = gosmData + idx[s++];
189          if (TagCmp (gosmData + idx[count + l], tag) != 0) break;
190          while (*--tag) {}
191          wayType *w = (wayType*)tag - 1;
192          if ((ZEnc ((unsigned)w->clat >> 16, (unsigned) w->clon >> 16) ^
193               cz ^ (mask & swap)) >> (2 + (bits << 1))) break;
194          __int64 d = Sqr ((__int64)(w->clat - clat)) +
195                      Sqr ((__int64)(w->clon - clon));
196          if (count < searchCnt || d < dista[count - 1]) {
197            if (count < searchCnt) count++;
198            for (ipos = count - 1; ipos > c && d < dista[ipos - 1]; ipos--) {
199              dista[ipos] = dista[ipos - 1];
200              gosmSway[ipos] = gosmSway[ipos - 1];
201              taga[ipos] = taga[ipos - 1];
202            }
203            gosmSway[ipos] = w;
204            dista[ipos] = d;
205            taga[ipos] = gosmData + idx[s - 1];
206          }
207        } // For each entry in the square
208      } // For each of the 4 squares
209      break; // count < searchCnt implies a bug. Don't loop infinitely.
210    } // If the search list is filled by tags with this text
211    count = c;
212  } // For each
213  for (int i = 0; i < searchCnt; i++) free (gosmSstr[i]);
214  for (int j = 0; j < count; j++) {
215    gosmSstr[j] = (char *) malloc (strcspn (taga[j], "\n") + 1);
216    sprintf (gosmSstr[j], "%.*s", (int) strcspn (taga[j], "\n"), taga[j]);
217  }
218  for (int k = count; k < searchCnt; k++) gosmSstr[k] = NULL;
219}
220
221/*------------------------------- OsmItr --------------------------------*/
222int Next (OsmItr &itr) /* Friend of osmItr */
223{
224  do {
225    itr.nd[0]++;
226    while (itr.nd[0] >= itr.end) {
227      if ((itr.slon += itr.tsize) == itr.right) {
228        itr.slon = itr.left;  /* Here we wrap around from N85 to S85 ! */
229        if ((itr.slat += itr.tsize) == itr.bottom) return FALSE;
230      }
231      int bucket = Hash (itr.slon, itr.slat, itr.tsize != TILESIZE);
232      itr.nd[0] = ndBase + hashTable[bucket];
233      itr.end = ndBase + hashTable[bucket + 1];
234    }
235  } while (((itr.nd[0]->lon ^ itr.slon) & (~(itr.tsize - 1))) ||
236           ((itr.nd[0]->lat ^ itr.slat) & (~(itr.tsize - 1))));
237/*      ((itr.hs[1] = (halfSegType *) (data + itr.hs[0]->other)) > itr.hs[0] &&
238       itr.left <= itr.hs[1]->lon && itr.hs[1]->lon < itr.right &&
239       itr.top <= itr.hs[1]->lat && itr.hs[1]->lat < itr.bottom)); */
240/* while nd[0] is a hash collision, */ 
241  return TRUE;
242}
243
244/* Routing starts at the 'to' point and moves to the 'from' point. This will
245   help when we do in car navigation because the 'from' point will change
246   often while the 'to' point stays fixed, so we can keep the array of nodes.
247   It also makes the generation of the directions easier.
248
249   We use "double hashing" to keep track of the shortest distance to each
250   node. So we guess an upper limit for the number of nodes that will be
251   considered and then multiply by a few so that there won't be too many
252   clashes. For short distances we allow for dense urban road networks,
253   but beyond a certain point there is bound to be farmland or seas.
254
255   We call nodes that rescently had their "best" increased "active". The
256   active nodes are stored in a heap so that we can quickly find the most
257   promissing one.
258   
259   OSM nodes are not our "graph-theor"etic nodes. Our "graph-theor"etic nodes
260   are "states", namely the ability to reach nd directly from nd->other[dir]
261*/
262#ifdef ROUTE_CALIBRATE
263int routeAddCnt;
264#define ROUTE_SET_ADDND_COUNT(x) routeAddCnt = (x)
265#define ROUTE_SHOW_STATS printf ("%d / %d\n", routeAddCnt, dhashSize); \
266  fprintf (stderr, "flat=%lf&flon=%lf&tlat=%lf&tlon=%lf&fast=%d&v=motorcar\n", \
267    LatInverse (flat), LonInverse (flon), LatInverse (tlat), \
268    LonInverse (tlon), fast)
269// This ratio must be around 0.5. Close to 0 or 1 is bad
270#else
271#define ROUTE_SET_ADDND_COUNT(x)
272#define ROUTE_SHOW_STATS
273#endif
274
275routeNodeType *AddNd (ndType *nd, int dir, int cost, routeNodeType *newshort)
276{ /* This function is called when we find a valid route that consists of the
277     segments (hs, hs->other), (newshort->hs, newshort->hs->other),
278     (newshort->shortest->hs, newshort->shortest->hs->other), .., 'to'
279     with cost 'cost'.
280     
281     When cost is -1 this function just returns the entry for nd without
282     modifying anything. */
283  unsigned hash = (intptr_t) nd / 10 + dir, i = 0;
284  routeNodeType *n;
285  do {
286    if (i++ > 10) {
287      //fprintf (stderr, "Double hash bailout : Table full, hash function "
288      //  "bad or no route exists\n");
289      return NULL;
290    }
291    n = route + hash % dhashSize;
292    /* Linear congruential generator from wikipedia */
293    hash = (unsigned) (hash * (__int64) 1664525 + 1013904223);
294    if (n->nd == NULL) { /* First visit of this node */
295      if (cost < 0) return NULL;
296      n->nd = nd;
297      n->best = 0x7fffffff;
298      /* Will do later : routeHeap[routeHeapSize] = n; */
299      n->heapIdx = routeHeapSize++;
300      n->dir = dir;
301      n->remain = lrint (sqrt (Sqr ((__int64)(nd->lat - rlat)) +
302                               Sqr ((__int64)(nd->lon - rlon))));
303      if (!shortest || n->remain < shortest->remain) shortest = n;
304      ROUTE_SET_ADDND_COUNT (routeAddCnt + 1);
305    }
306  } while (n->nd != nd || n->dir != dir);
307
308  int diff = n->remain + (newshort ? newshort->best - newshort->remain : 0);
309  if (cost >= 0 && n->best > cost + diff) {
310    n->best = cost + diff;
311    n->shortest = newshort;
312    if (n->heapIdx < 0) n->heapIdx = routeHeapSize++;
313    for (; n->heapIdx > 1 &&
314         n->best < routeHeap[n->heapIdx / 2]->best; n->heapIdx /= 2) {
315      routeHeap[n->heapIdx] = routeHeap[n->heapIdx / 2];
316      routeHeap[n->heapIdx]->heapIdx = n->heapIdx;
317    }
318    routeHeap[n->heapIdx] = n;
319  }
320  return n;
321}
322
323inline int IsOneway (wayType *w, int Vehicle)
324{
325  return Vehicle != footR && Vehicle != bicycleR && (w->bits & (1<<onewayR));
326}
327
328void Route (int recalculate, int plon, int plat, int Vehicle, int fast)
329{ /* Recalculate is faster but only valid if 'to', 'Vehicle' and
330     'fast' did not change */
331/* We start by finding the segment that is closest to 'from' and 'to' */
332  static ndType *endNd[2] = { NULL, NULL}, from;
333  static int toEndNd[2][2];
334 
335  ROUTE_SET_ADDND_COUNT (0);
336  shortest = NULL;
337  for (int i = recalculate ? 0 : 1; i < 2; i++) {
338    int lon = i ? flon : tlon, lat = i ? flat : tlat;
339    __int64 bestd = (__int64) 1 << 62;
340    /* find min (Sqr (distance)). Use long long so we don't loose accuracy */
341    OsmItr itr (lon - 100000, lat - 100000, lon + 100000, lat + 100000);
342    /* Search 1km x 1km around 'from' for the nearest segment to it */
343    while (Next (itr)) {
344      // We don't do for (int dir = 0; dir < 1; dir++) {
345      // because if our search box is large enough, it will also give us
346      // the other node.
347      if (!(Way (itr.nd[0])->bits & (1 << Vehicle))) {
348        continue;
349      }
350      if (itr.nd[0]->other[0] < 0) continue;
351      __int64 lon0 = lon - itr.nd[0]->lon, lat0 = lat - itr.nd[0]->lat,
352              lon1 = lon - (ndBase + itr.nd[0]->other[0])->lon,
353              lat1 = lat - (ndBase + itr.nd[0]->other[0])->lat,
354              dlon = lon1 - lon0, dlat = lat1 - lat0;
355      /* We use Pythagoras to test angles for being greater that 90 and
356         consequently if the point is behind hs[0] or hs[1].
357         If the point is "behind" hs[0], measure distance to hs[0] with
358         Pythagoras. If it's "behind" hs[1], use Pythagoras to hs[1]. If
359         neither, use perpendicular distance from a point to a line */
360      int segLen = lrint (sqrt ((double)(Sqr(dlon) + Sqr (dlat))));
361      __int64 d = dlon * lon0 >= - dlat * lat0 ? Sqr (lon0) + Sqr (lat0) :
362        dlon * lon1 <= - dlat * lat1 ? Sqr (lon1) + Sqr (lat1) :
363        Sqr ((dlon * lat1 - dlat * lon1) / segLen);
364     
365      wayType *w = Way (itr.nd[0]);
366      if (i) { // For 'from' we take motion into account
367        __int64 motion = segLen ? 3 * (dlon * plon + dlat * plat) / segLen
368          : 0;
369        // What is the most appropriate multiplier for motion ?
370        if (motion > 0 && IsOneway (w, Vehicle)) d += Sqr (motion);
371        else d -= Sqr (motion);
372        // Is it better to say :
373        // d = lrint (sqrt ((double) d));
374        // if (motion < 0 || IsOneway (w)) d += motion;
375        // else d -= motion;
376      }
377     
378      if (d < bestd) {
379        bestd = d;
380        double invSpeed = !fast ? 1.0 : Style (w)->invSpeed[Vehicle];
381        //printf ("%d %lf\n", i, invSpeed);
382        toEndNd[i][0] =
383          lrint (sqrt ((double)(Sqr (lon0) + Sqr (lat0))) * invSpeed);
384        toEndNd[i][1] =
385          lrint (sqrt ((double)(Sqr (lon1) + Sqr (lat1))) * invSpeed);
386//        if (dlon * lon1 <= -dlat * lat1) toEndNd[i][1] += toEndNd[i][0] * 9;
387//        if (dlon * lon0 >= -dlat * lat0) toEndNd[i][0] += toEndNd[i][1] * 9;
388
389        if (IsOneway (w, Vehicle)) toEndNd[i][1 - i] = 200000000;
390        /*  It's possible to go up a oneway at the end, but at a huge penalty*/
391        endNd[i] = itr.nd[0];
392        /* The router only stops after it has traversed endHs[1], so if we
393           want 'limit' to be accurate, we must subtract it's length
394        if (i) {
395          toEndHs[1][0] -= segLen;
396          toEndHs[1][1] -= segLen;
397        } */
398      }
399    } /* For each candidate segment */
400    if (bestd == ((__int64) 1 << 62) || !endNd[0]) {
401      endNd[i] = NULL;
402      //fprintf (stderr, "No segment nearby\n");
403      return;
404    }
405  } /* For 'from' and 'to', find segment that passes nearby */
406  from.lat = flat;
407  from.lon = flon;
408  if (recalculate || !route) {
409    free (route);
410    dhashSize = Sqr ((tlon - flon) >> 16) + Sqr ((tlat - flat) >> 16) + 20;
411    dhashSize = dhashSize < 10000 ? dhashSize * 1000 : 10000000;
412    // Allocate one piece of memory for both route and routeHeap, so that
413    // we can easily retry if it fails on a small device
414    #ifdef _WIN32_WCE
415    MEMORYSTATUS memStat;
416    GlobalMemoryStatus (&memStat);
417    int lim = (memStat.dwAvailPhys - 1400000) / // Leave 1.4 MB free
418                 (sizeof (*route) + sizeof (*routeHeap));
419    if (dhashSize > lim && lim > 0) dhashSize = lim;
420    #endif
421
422    while (dhashSize > 0 && !(route = (routeNodeType*)
423        malloc ((sizeof (*route) + sizeof (*routeHeap)) * dhashSize))) {
424      dhashSize = dhashSize / 4 * 3;
425    }
426    memset (route, 0, sizeof (dhashSize) * dhashSize);
427    routeHeapSize = 1; /* Leave position 0 open to simplify the math */
428    routeHeap = (routeNodeType**) (route + dhashSize) - 1;
429
430    rlat = flat;
431    rlon = flon;
432    AddNd (endNd[0], 0, toEndNd[0][0], NULL);
433    AddNd (ndBase + endNd[0]->other[0], 1, toEndNd[0][1], NULL);
434    AddNd (endNd[0], 1, toEndNd[0][0], NULL);
435    AddNd (ndBase + endNd[0]->other[0], 0, toEndNd[0][1], NULL);
436  }
437  else {
438    routeNodeType *frn = AddNd (&from, 0, -1, NULL);
439    if (frn) frn->best = 0x7fffffff;
440
441    routeNodeType *rn = AddNd (endNd[1], 0, -1, NULL);
442    if (rn) AddNd (&from, 0, toEndNd[1][1], rn);
443    routeNodeType *rno = AddNd (ndBase + endNd[1]->other[0], 1, -1, NULL);
444    if (rno) AddNd (&from, 0, toEndNd[1][0], rno);
445  }
446 
447  while (routeHeapSize > 1) {
448    routeNodeType *root = routeHeap[1];
449    routeHeapSize--;
450    int beste = routeHeap[routeHeapSize]->best;
451    for (int i = 2; ; ) {
452      int besti = i < routeHeapSize ? routeHeap[i]->best : beste;
453      int bestipp = i + 1 < routeHeapSize ? routeHeap[i + 1]->best : beste;
454      if (besti > bestipp) i++;
455      else bestipp = besti;
456      if (beste <= bestipp) {
457        routeHeap[i / 2] = routeHeap[routeHeapSize];
458        routeHeap[i / 2]->heapIdx = i / 2;
459        break;
460      }
461      routeHeap[i / 2] = routeHeap[i];
462      routeHeap[i / 2]->heapIdx = i / 2;
463      i = i * 2;
464    }
465    root->heapIdx = -1; /* Root now removed from the heap */
466    if (root->nd == &from) { // Remove 'from' from the heap in case we
467      shortest = root->shortest; // get called with recalculate=0
468      break;
469    }
470    if (root->nd == (!root->dir ? endNd[1] : ndBase + endNd[1]->other[0])) {
471      AddNd (&from, 0, toEndNd[1][1 - root->dir], root);
472    }
473    ndType *nd = root->nd, *other, *firstNd, *restrictItr;
474    while (nd > ndBase && nd[-1].lon == nd->lon &&
475      nd[-1].lat == nd->lat) nd--; /* Find first nd in node */
476    firstNd = nd; // Save it for checking restrictions
477    /* Now work through the segments connected to root. */
478    do {
479//          if (StyleNr ((wayType *)(data + nd->wayPtr)) >= restriction_no_right_turn) printf ("%d\n", nd - firstNd);
480      for (int dir = 0; dir < 2; dir++) {
481        if (nd == root->nd && dir == root->dir) continue;
482        /* Don't consider an immediate U-turn to reach root->hs->other.
483           This doesn't exclude 179.99 degree turns though. */
484       
485        if (nd->other[dir] < 0) continue;
486        if (Vehicle != footR && Vehicle != bicycleR) {
487          for (restrictItr = firstNd; restrictItr->other[0] < 0 &&
488                          restrictItr->other[1] < 0; restrictItr++) {
489            wayType *= Way (restrictItr);
490            if (StyleNr (w) < restriction_no_right_turn ||
491                StyleNr (w) > restriction_only_straight_on) continue;
492  //          printf ("aa\n");
493            if (atoi ((char*)(w + 1) + 1) == nd->wayPtr &&
494                atoi (strchr ((char*)(w + 1) + 1, ' ')) == root->nd->wayPtr) {
495               ndType *n2 = ndBase + root->nd->other[root->dir];
496               ndType *n0 = ndBase + nd->other[dir];
497               __int64 straight =
498                 (n2->lat - nd->lat) * (__int64)(nd->lat - n0->lat) +
499                 (n2->lon - nd->lon) * (__int64)(nd->lon - n0->lon), left =
500                 (n2->lat - nd->lat) * (__int64)(nd->lon - n0->lon) -
501                 (n2->lon - nd->lon) * (__int64)(nd->lat - n0->lat);
502               int azi = straight < left ? (straight < -left ? 3 : 0) :
503                 straight < -left ? 2 : 1;
504//               printf ("%d %9d %9d %d %d\n", azi, n2->lon - nd->lon, n0->lon - nd->lon, straight < left, straight < -left);
505//               printf ("%d %9d %9d\n", azi, n2->lat - nd->lat, n0->lat - nd->lat);
506               static const int no[] = { restriction_no_left_turn,
507                 restriction_no_straight_on, restriction_no_right_turn,
508                 restriction_no_u_turn },
509                 only[] = { restriction_only_left_turn,
510                 restriction_only_straight_on, restriction_only_right_turn,
511                 -1 /*  restriction_only_u_turn */ };
512               if (StyleNr (w) == only[azi ^ 1] ||
513                   StyleNr (w) == only[azi ^ 2] || StyleNr (w) == only[azi ^ 3]
514                   || StyleNr (w) == no[azi]) break;
515//               printf ("%d %d %d\n", azi, n2->lon, n0->lon);
516            }
517          }
518          if (restrictItr->other[0] < 0 &&
519              restrictItr->other[1] < 0) continue;
520        }
521        // Tagged node, start or end of way or restriction.
522       
523        other = ndBase + nd->other[dir];
524        wayType *w = Way (nd);
525        if ((w->bits & (1 << Vehicle)) && (dir || !IsOneway (w, Vehicle))) {
526          int d = lrint (sqrt ((double)
527            (Sqr ((__int64)(nd->lon - other->lon)) +
528             Sqr ((__int64)(nd->lat - other->lat)))) *
529                        (fast ? Style (w)->invSpeed[Vehicle] : 1.0));                 
530          AddNd (other, 1 - dir, d, root);
531        } // If we found a segment we may follow
532      }
533    } while (++nd < ndBase + hashTable[bucketsMin1 + 1] &&
534             nd->lon == nd[-1].lon && nd->lat == nd[-1].lat);
535  } // While there are active nodes left
536  ROUTE_SHOW_STATS;
537//  if (fastest) printf ("%lf
538//  printf ("%lf km\n", limit / 100000.0);
539}
540
541int JunctionType (ndType *nd)
542{
543  int ret = 'j';
544  while (nd > ndBase && nd[-1].lon == nd->lon &&
545    nd[-1].lat == nd->lat) nd--;
546  int segCnt = 0; // Count number of segments at x->shortest
547  do {
548    // TODO : Only count segment traversable by 'Vehicle'
549    // Except for the case where a cyclist passes a motorway_link.
550    // TODO : Don't count oneways entering the roundabout
551    if (nd->other[0] >= 0) segCnt++;
552    if (nd->other[1] >= 0) segCnt++;
553    if (StyleNr (Way (nd)) == highway_traffic_signals) {
554      ret = 't';
555    }
556    if (StyleNr (Way (nd)) == highway_mini_roundabout) {
557      ret = 'm';
558    }   
559  } while (++nd < ndBase + hashTable[bucketsMin1 + 1] &&
560           nd->lon == nd[-1].lon && nd->lat == nd[-1].lat);
561  return segCnt > 2 ? toupper (ret) : ret;
562}
563
564
565int GosmInit (void *d, long size)
566{
567  if (!d) return FALSE;
568  gosmData = (char*) d;
569  bucketsMin1 = ((int *) (gosmData + size))[-2];
570  hashTable = (int *) (gosmData + size) - bucketsMin1 - (bucketsMin1 >> 7)
571                      - 5;
572  ndBase = (ndType *)(gosmData + hashTable[bucketsMin1 + (bucketsMin1 >> 7)
573     + 4]);
574  style = (struct styleStruct *)(gosmData + 4);
575  memset (gosmSway, 0, sizeof (gosmSway));
576  return ndBase && hashTable && *(int*) gosmData == pakHead;
577}
Note: See TracBrowser for help on using the repository browser.