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

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