git.maemo.org Git - routino/blob - src/nodes.c

   1 /***************************************
   2  $Header: /home/amb/routino/src/RCS/nodes.c,v 1.39 2010/07/08 17:54:54 amb Exp $
   3
   4  Node data type functions.
   5
   6  Part of the Routino routing software.
   7  ******************/ /******************
   8  This file Copyright 2008-2010 Andrew M. Bishop
   9
  10  This program is free software: you can redistribute it and/or modify
  11  it under the terms of the GNU Affero General Public License as published by
  12  the Free Software Foundation, either version 3 of the License, or
  13  (at your option) any later version.
  14
  15  This program is distributed in the hope that it will be useful,
  16  but WITHOUT ANY WARRANTY; without even the implied warranty of
  17  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  18  GNU Affero General Public License for more details.
  19
  20  You should have received a copy of the GNU Affero General Public License
  21  along with this program.  If not, see <http://www.gnu.org/licenses/>.
  22  ***************************************/
  23
  24
  25 #include <sys/types.h>
  26 #include <stdlib.h>
  27 #include <math.h>
  28
  29 #include "profiles.h"
  30 #include "nodes.h"
  31 #include "segments.h"
  32 #include "ways.h"
  33 #include "functions.h"
  34
  35
  36 /*++++++++++++++++++++++++++++++++++++++
  37   Load in a node list from a file.
  38
  39   Nodes* LoadNodeList Returns the node list.
  40
  41   const char *filename The name of the file to load.
  42   ++++++++++++++++++++++++++++++++++++++*/
  43
  44 Nodes *LoadNodeList(const char *filename)
  45 {
  46  void *data;
  47  Nodes *nodes;
  48
  49  nodes=(Nodes*)malloc(sizeof(Nodes));
  50
  51  data=MapFile(filename);
  52
  53  /* Copy the Nodes structure from the loaded data */
  54
  55  *nodes=*((Nodes*)data);
  56
  57  /* Adjust the pointers in the Nodes structure. */
  58
  59  nodes->data=data;
  60  nodes->offsets=(index_t*)(data+sizeof(Nodes));
  61  nodes->nodes=(Node*)(data+(sizeof(Nodes)+(nodes->latbins*nodes->lonbins+1)*sizeof(index_t)));
  62
  63  return(nodes);
  64 }
  65
  66
  67 /*++++++++++++++++++++++++++++++++++++++
  68   Find the closest node given its latitude, longitude and optionally profile.
  69
  70   index_t FindClosestNode Returns the closest node.
  71
  72   Nodes* nodes The set of nodes to search.
  73
  74   Segments *segments The set of segments to use.
  75
  76   Ways *ways The set of ways to use.
  77
  78   double latitude The latitude to look for.
  79
  80   double longitude The longitude to look for.
  81
  82   distance_t distance The maximum distance to look.
  83
  84   Profile *profile The profile of the mode of transport (or NULL).
  85
  86   distance_t *bestdist Returns the distance to the best node.
  87   ++++++++++++++++++++++++++++++++++++++*/
  88
  89 index_t FindClosestNode(Nodes* nodes,Segments *segments,Ways *ways,double latitude,double longitude,
  90                         distance_t distance,Profile *profile,distance_t *bestdist)
  91 {
  92  ll_bin_t   latbin=latlong_to_bin(radians_to_latlong(latitude ))-nodes->latzero;
  93  ll_bin_t   lonbin=latlong_to_bin(radians_to_latlong(longitude))-nodes->lonzero;
  94  int        delta=0,count;
  95  index_t    i,bestn=NO_NODE;
  96  distance_t bestd=INF_DISTANCE;
  97
  98  /* Start with the bin containing the location, then spiral outwards. */
  99
 100  do
 101    {
 102     int latb,lonb,llbin;
 103
 104     count=0;
 105
 106     for(latb=latbin-delta;latb<=latbin+delta;latb++)
 107       {
 108        if(latb<0 || latb>=nodes->latbins)
 109           continue;
 110
 111        for(lonb=lonbin-delta;lonb<=lonbin+delta;lonb++)
 112          {
 113           if(lonb<0 || lonb>=nodes->lonbins)
 114              continue;
 115
 116           if(abs(latb-latbin)<delta && abs(lonb-lonbin)<delta)
 117              continue;
 118
 119           llbin=lonb*nodes->latbins+latb;
 120
 121           /* Check if this grid square has any hope of being close enough */
 122
 123           if(delta>0)
 124             {
 125              double lat1=latlong_to_radians(bin_to_latlong(nodes->latzero+latb));
 126              double lon1=latlong_to_radians(bin_to_latlong(nodes->lonzero+lonb));
 127              double lat2=latlong_to_radians(bin_to_latlong(nodes->latzero+latb+1));
 128              double lon2=latlong_to_radians(bin_to_latlong(nodes->lonzero+lonb+1));
 129
 130              if(latb==latbin)
 131                {
 132                 distance_t dist1=Distance(latitude,lon1,latitude,longitude);
 133                 distance_t dist2=Distance(latitude,lon2,latitude,longitude);
 134
 135                 if(dist1>distance && dist2>distance)
 136                    continue;
 137                }
 138              else if(lonb==lonbin)
 139                {
 140                 distance_t dist1=Distance(lat1,longitude,latitude,longitude);
 141                 distance_t dist2=Distance(lat2,longitude,latitude,longitude);
 142
 143                 if(dist1>distance && dist2>distance)
 144                    continue;
 145                }
 146              else
 147                {
 148                 distance_t dist1=Distance(lat1,lon1,latitude,longitude);
 149                 distance_t dist2=Distance(lat2,lon1,latitude,longitude);
 150                 distance_t dist3=Distance(lat2,lon2,latitude,longitude);
 151                 distance_t dist4=Distance(lat1,lon2,latitude,longitude);
 152
 153                 if(dist1>distance && dist2>distance && dist3>distance && dist4>distance)
 154                    continue;
 155                }
 156             }
 157
 158           /* Check every node in this grid square. */
 159
 160           for(i=nodes->offsets[llbin];i<nodes->offsets[llbin+1];i++)
 161             {
 162              double lat=latlong_to_radians(bin_to_latlong(nodes->latzero+latb)+off_to_latlong(nodes->nodes[i].latoffset));
 163              double lon=latlong_to_radians(bin_to_latlong(nodes->lonzero+lonb)+off_to_latlong(nodes->nodes[i].lonoffset));
 164
 165              distance_t dist=Distance(lat,lon,latitude,longitude);
 166
 167              if(dist<distance)
 168                {
 169                 if(profile)
 170                   {
 171                    Segment *segment;
 172
 173                    /* Decide if this is node is valid for the profile */
 174
 175                    segment=FirstSegment(segments,nodes,i);
 176
 177                    do
 178                      {
 179                       Way *way=LookupWay(ways,segment->way);
 180
 181                       if(way->allow&profile->allow)
 182                          break;
 183
 184                       segment=NextSegment(segments,segment,i);
 185                      }
 186                    while(segment);
 187
 188                    if(!segment)
 189                       continue;
 190                   }
 191
 192                 bestn=i; bestd=distance=dist;
 193                }
 194             }
 195
 196           count++;
 197          }
 198       }
 199
 200     delta++;
 201    }
 202  while(count);
 203
 204  *bestdist=bestd;
 205
 206  return(bestn);
 207 }
 208
 209
 210 /*++++++++++++++++++++++++++++++++++++++
 211   Find the closest segment to a latitude, longitude and optionally profile.
 212
 213   Segment *FindClosestSegment Returns the closest segment.
 214
 215   Nodes* nodes The set of nodes to search.
 216
 217   Segments *segments The set of segments to use.
 218
 219   Ways *ways The set of ways to use.
 220
 221   double latitude The latitude to look for.
 222
 223   double longitude The longitude to look for.
 224
 225   distance_t distance The maximum distance to look.
 226
 227   Profile *profile The profile of the mode of transport (or NULL).
 228
 229   distance_t *bestdist Returns the distance to the best segment.
 230
 231   index_t *bestnode1 Returns the best node at one end.
 232
 233   index_t *bestnode2 Returns the best node at the other end.
 234
 235   distance_t *bestdist1 Returns the distance to the best node at one end.
 236
 237   distance_t *bestdist2 Returns the distance to the best node at the other end.
 238   ++++++++++++++++++++++++++++++++++++++*/
 239
 240 Segment *FindClosestSegment(Nodes* nodes,Segments *segments,Ways *ways,double latitude,double longitude,
 241                             distance_t distance,Profile *profile, distance_t *bestdist,
 242                             index_t *bestnode1,index_t *bestnode2,distance_t *bestdist1,distance_t *bestdist2)
 243 {
 244  ll_bin_t   latbin=latlong_to_bin(radians_to_latlong(latitude ))-nodes->latzero;
 245  ll_bin_t   lonbin=latlong_to_bin(radians_to_latlong(longitude))-nodes->lonzero;
 246  int        delta=0,count;
 247  index_t    i,bestn1=NO_NODE,bestn2=NO_NODE;
 248  distance_t bestd=INF_DISTANCE,bestd1=INF_DISTANCE,bestd2=INF_DISTANCE;
 249  Segment   *bests=NULL;
 250
 251  /* Start with the bin containing the location, then spiral outwards. */
 252
 253  do
 254    {
 255     int latb,lonb,llbin;
 256
 257     count=0;
 258
 259     for(latb=latbin-delta;latb<=latbin+delta;latb++)
 260       {
 261        if(latb<0 || latb>=nodes->latbins)
 262           continue;
 263
 264        for(lonb=lonbin-delta;lonb<=lonbin+delta;lonb++)
 265          {
 266           if(lonb<0 || lonb>=nodes->lonbins)
 267              continue;
 268
 269           if(abs(latb-latbin)<delta && abs(lonb-lonbin)<delta)
 270              continue;
 271
 272           llbin=lonb*nodes->latbins+latb;
 273
 274           /* Check if this grid square has any hope of being close enough */
 275
 276           if(delta>0)
 277             {
 278              double lat1=latlong_to_radians(bin_to_latlong(nodes->latzero+latb));
 279              double lon1=latlong_to_radians(bin_to_latlong(nodes->lonzero+lonb));
 280              double lat2=latlong_to_radians(bin_to_latlong(nodes->latzero+latb+1));
 281              double lon2=latlong_to_radians(bin_to_latlong(nodes->lonzero+lonb+1));
 282
 283              if(latb==latbin)
 284                {
 285                 distance_t dist1=Distance(latitude,lon1,latitude,longitude);
 286                 distance_t dist2=Distance(latitude,lon2,latitude,longitude);
 287
 288                 if(dist1>distance && dist2>distance)
 289                    continue;
 290                }
 291              else if(lonb==lonbin)
 292                {
 293                 distance_t dist1=Distance(lat1,longitude,latitude,longitude);
 294                 distance_t dist2=Distance(lat2,longitude,latitude,longitude);
 295
 296                 if(dist1>distance && dist2>distance)
 297                    continue;
 298                }
 299              else
 300                {
 301                 distance_t dist1=Distance(lat1,lon1,latitude,longitude);
 302                 distance_t dist2=Distance(lat2,lon1,latitude,longitude);
 303                 distance_t dist3=Distance(lat2,lon2,latitude,longitude);
 304                 distance_t dist4=Distance(lat1,lon2,latitude,longitude);
 305
 306                 if(dist1>distance && dist2>distance && dist3>distance && dist4>distance)
 307                    continue;
 308                }
 309             }
 310
 311           /* Check every node in this grid square. */
 312
 313           for(i=nodes->offsets[llbin];i<nodes->offsets[llbin+1];i++)
 314             {
 315              double lat1=latlong_to_radians(bin_to_latlong(nodes->latzero+latb)+off_to_latlong(nodes->nodes[i].latoffset));
 316              double lon1=latlong_to_radians(bin_to_latlong(nodes->lonzero+lonb)+off_to_latlong(nodes->nodes[i].lonoffset));
 317              distance_t dist1;
 318
 319              dist1=Distance(lat1,lon1,latitude,longitude);
 320
 321              if(dist1<distance)
 322                {
 323                 Segment *segment;
 324
 325                 /* Check each segment for closeness and if valid for the profile */
 326
 327                 segment=FirstSegment(segments,nodes,i);
 328
 329                 do
 330                   {
 331                    if(IsNormalSegment(segment))
 332                      {
 333                       Way *way=NULL;
 334
 335                       if(profile)
 336                          way=LookupWay(ways,segment->way);
 337
 338                       if(!profile || way->allow&profile->allow)
 339                         {
 340                          distance_t dist2,dist3;
 341                          double lat2,lon2,dist3a,dist3b,distp;
 342
 343                          GetLatLong(nodes,OtherNode(segment,i),&lat2,&lon2);
 344
 345                          dist2=Distance(lat2,lon2,latitude,longitude);
 346
 347                          dist3=Distance(lat1,lon1,lat2,lon2);
 348
 349                          /* Use law of cosines (assume flat Earth) */
 350
 351                          dist3a=((double)dist1*(double)dist1-(double)dist2*(double)dist2+(double)dist3*(double)dist3)/(2.0*(double)dist3);
 352                          dist3b=(double)dist3-dist3a;
 353
 354                          if(dist3a>=0 && dist3b>=0)
 355                             distp=sqrt((double)dist1*(double)dist1-dist3a*dist3a);
 356                          else if(dist3a>0)
 357                            {
 358                             distp=dist2;
 359                             dist3a=dist3;
 360                             dist3b=0;
 361                            }
 362                          else /* if(dist3b>0) */
 363                            {
 364                             distp=dist1;
 365                             dist3a=0;
 366                             dist3b=dist3;
 367                            }
 368
 369                          if(distp<(double)bestd)
 370                            {
 371                             bests=segment;
 372                             bestn1=i;
 373                             bestn2=OtherNode(segment,i);
 374                             bestd1=(distance_t)dist3a;
 375                             bestd2=(distance_t)dist3b;
 376                             bestd=(distance_t)distp;
 377                            }
 378                         }
 379                      }
 380
 381                    segment=NextSegment(segments,segment,i);
 382                   }
 383                 while(segment);
 384                }
 385
 386             } /* dist1 < distance */
 387
 388           count++;
 389          }
 390       }
 391
 392     delta++;
 393    }
 394  while(count);
 395
 396  *bestdist=bestd;
 397
 398  *bestnode1=bestn1;
 399  *bestnode2=bestn2;
 400  *bestdist1=bestd1;
 401  *bestdist2=bestd2;
 402
 403  return(bests);
 404 }
 405
 406
 407 /*++++++++++++++++++++++++++++++++++++++
 408   Get the latitude and longitude associated with a node.
 409
 410   Nodes *nodes The set of nodes.
 411
 412   index_t index The node index.
 413
 414   double *latitude Returns the latitude.
 415
 416   double *longitude Returns the logitude.
 417   ++++++++++++++++++++++++++++++++++++++*/
 418
 419 void GetLatLong(Nodes *nodes,index_t index,double *latitude,double *longitude)
 420 {
 421  Node *node=&nodes->nodes[index];
 422  int latbin=-1,lonbin=-1;
 423  int start,end,mid;
 424
 425  /* Binary search - search key closest below is required.
 426   *
 427   *  # <- start  |  Check mid and move start or end if it doesn't match
 428   *  #           |
 429   *  #           |  Since an inexact match is wanted we must set end=mid-1
 430   *  # <- mid    |  or start=mid because we know that mid doesn't match.
 431   *  #           |
 432   *  #           |  Eventually either end=start or end=start+1 and one of
 433   *  # <- end    |  start or end is the wanted one.
 434   */
 435
 436  /* Search for longitude */
 437
 438  start=0;
 439  end=nodes->lonbins-1;
 440
 441  do
 442    {
 443     mid=(start+end)/2;                                 /* Choose mid point */
 444
 445     if(nodes->offsets[nodes->latbins*mid]<index)       /* Mid point is too low */
 446        start=mid;
 447     else if(nodes->offsets[nodes->latbins*mid]>index)  /* Mid point is too high */
 448        end=mid-1;
 449     else                                               /* Mid point is correct */
 450       {lonbin=mid;break;}
 451    }
 452  while((end-start)>1);
 453
 454  if(lonbin==-1)
 455    {
 456     if(nodes->offsets[nodes->latbins*end]>index)
 457        lonbin=start;
 458     else
 459        lonbin=end;
 460    }
 461
 462  while(lonbin<nodes->lonbins && nodes->offsets[lonbin*nodes->latbins]==nodes->offsets[(lonbin+1)*nodes->latbins])
 463     lonbin++;
 464
 465  /* Search for latitude */
 466
 467  start=0;
 468  end=nodes->latbins-1;
 469
 470  do
 471    {
 472     mid=(start+end)/2;                                       /* Choose mid point */
 473
 474     if(nodes->offsets[lonbin*nodes->latbins+mid]<index)      /* Mid point is too low */
 475        start=mid;
 476     else if(nodes->offsets[lonbin*nodes->latbins+mid]>index) /* Mid point is too high */
 477        end=mid-1;
 478     else                                                     /* Mid point is correct */
 479       {latbin=mid;break;}
 480    }
 481  while((end-start)>1);
 482
 483  if(latbin==-1)
 484    {
 485     if(nodes->offsets[lonbin*nodes->latbins+end]>index)
 486        latbin=start;
 487     else
 488        latbin=end;
 489    }
 490
 491  while(latbin<nodes->latbins && nodes->offsets[lonbin*nodes->latbins+latbin]==nodes->offsets[lonbin*nodes->latbins+latbin+1])
 492     latbin++;
 493
 494  /* Return the values */
 495
 496  *latitude =latlong_to_radians(bin_to_latlong(nodes->latzero+latbin)+off_to_latlong(node->latoffset));
 497  *longitude=latlong_to_radians(bin_to_latlong(nodes->lonzero+lonbin)+off_to_latlong(node->lonoffset));
 498 }