src/zonedetect.c

   1 /*
   2  * Copyright (c) 2018, Bertold Van den Bergh (vandenbergh@bertold.org)
   3  * All rights reserved.
   4  *
   5  * Redistribution and use in source and binary forms, with or without
   6  * modification, are permitted provided that the following conditions are met:
   7  *     * Redistributions of source code must retain the above copyright
   8  *       notice, this list of conditions and the following disclaimer.
   9  *     * Redistributions in binary form must reproduce the above copyright
  10  *       notice, this list of conditions and the following disclaimer in the
  11  *       documentation and/or other materials provided with the distribution.
  12  *     * Neither the name of the author nor the
  13  *       names of its contributors may be used to endorse or promote products
  14  *       derived from this software without specific prior written permission.
  15  *
  16  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
  17  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  18  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  19  * DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR DISTRIBUTOR BE LIABLE FOR ANY
  20  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  21  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  22  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
  23  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  24  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  25  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  26  */
  27
  28 #include <sys/mman.h>
  29 #include <stdlib.h>
  30 #include <string.h>
  31 #include <fcntl.h>
  32 #include <unistd.h>
  33 #include <math.h>
  34
  35 #include "zonedetect.h"
  36
  37 struct ZoneDetectOpaque {
  38     int fd;
  39     uint32_t length;
  40     uint8_t* mapping;
  41
  42     uint8_t tableType;
  43     uint8_t version;
  44     uint8_t precision;
  45     uint8_t numFields;
  46
  47     char* notice;
  48     char** fieldNames;
  49
  50     uint32_t bboxOffset;
  51     uint32_t metadataOffset;
  52     uint32_t dataOffset;
  53 };
  54
  55 static int32_t ZDFloatToFixedPoint(float input, float scale, unsigned int precision) {
  56     float inputScaled = input / scale;
  57     return inputScaled * (float)(1 << (precision-1));
  58 }
  59
  60 static unsigned int ZDDecodeVariableLengthUnsigned(ZoneDetect* library, uint32_t* index, uint32_t* result) {
  61     uint32_t value = 0;
  62     unsigned int i=0, shift = 0;
  63
  64     if(*index >= library->length) {
  65         return 0;
  66     }
  67
  68     uint8_t* buffer = library->mapping + *index;
  69     uint8_t* bufferEnd = library->mapping + library->length - 1;
  70
  71     while(1) {
  72         value |= (buffer[i] & 0x7F) << shift;
  73         shift += 7;
  74
  75         if(!(buffer[i] & 0x80)) {
  76             break;
  77         }
  78
  79         i++;
  80         if(buffer + i > bufferEnd) {
  81             return 0;
  82         }
  83     }
  84
  85     i++;
  86     *result = value;
  87     *index += i;
  88     return i;
  89 }
  90
  91 static unsigned int ZDDecodeVariableLengthSigned(ZoneDetect* library, uint32_t* index, int32_t* result) {
  92     uint32_t value = 0;
  93     unsigned int retVal = ZDDecodeVariableLengthUnsigned(library, index, &value);
  94     *result = (value & 1)?-(value/2):(value/2);
  95     return retVal;
  96 }
  97
  98 static char* ZDParseString(ZoneDetect* library, uint32_t* index) {
  99     uint32_t strLength;
 100     if(!ZDDecodeVariableLengthUnsigned(library, index, &strLength)) {
 101         return NULL;
 102     }
 103
 104     uint32_t strOffset = *index;
 105     unsigned int remoteStr = 0;
 106     if(strLength >= 256) {
 107         strOffset = library->metadataOffset + strLength - 256;
 108         remoteStr = 1;
 109
 110         if(!ZDDecodeVariableLengthUnsigned(library, &strOffset, &strLength)) {
 111             return NULL;
 112         }
 113
 114         if(strLength > 256) {
 115             return NULL;
 116         }
 117     }
 118
 119     char* str = malloc(strLength + 1);
 120
 121     if(str) {
 122         unsigned int i;
 123         for(i=0; i<strLength; i++) {
 124             str[i] = library->mapping[strOffset+i] ^ 0x80;
 125         }
 126         str[strLength] = 0;
 127     }
 128
 129     if(!remoteStr) {
 130         *index += strLength;
 131     }
 132
 133     return str;
 134 }
 135
 136 static int ZDParseHeader(ZoneDetect* library) {
 137     if(library->length < 7) {
 138         return -1;
 139     }
 140
 141     if(memcmp(library->mapping, "PLB", 3)) {
 142         return -1;
 143     }
 144
 145     library->tableType = library->mapping[3];
 146     library->version   = library->mapping[4];
 147     library->precision = library->mapping[5];
 148     library->numFields = library->mapping[6];
 149
 150     if(library->version != 0) {
 151         return -1;
 152     }
 153
 154     uint32_t index = 7;
 155
 156     library->fieldNames = malloc(library->numFields * sizeof(char*));
 157     unsigned int i;
 158     for(i=0; i<library->numFields; i++) {
 159         library->fieldNames[i] = ZDParseString(library, &index);
 160     }
 161
 162     library->notice = ZDParseString(library, &index);
 163     if(!library->notice) {
 164         return -1;
 165     }
 166
 167     uint32_t tmp;
 168     /* Read section sizes */
 169     /* By memset: library->bboxOffset = 0 */
 170
 171     if(!ZDDecodeVariableLengthUnsigned(library, &index, &tmp)) return -1;
 172     library->metadataOffset = tmp + library->bboxOffset;
 173
 174     if(!ZDDecodeVariableLengthUnsigned(library, &index, &tmp))return -1;
 175     library->dataOffset = tmp + library->metadataOffset;
 176
 177     if(!ZDDecodeVariableLengthUnsigned(library, &index, &tmp)) return -1;
 178
 179     /* Add header size to everything */
 180     library->bboxOffset += index;
 181     library->metadataOffset += index;
 182     library->dataOffset += index;
 183
 184     /* Verify file length */
 185     if(tmp + library->dataOffset != library->length) {
 186         return -2;
 187     }
 188
 189     return 0;
 190 }
 191
 192 static int ZDPointInBox(int32_t xl, int32_t x, int32_t xr, int32_t yl, int32_t y, int32_t yr) {
 193     if((xl <= x && x <= xr) || (xr <= x && x <= xl)) {
 194         if((yl <= y && y <= yr) || (yr <= y && y <= yl)) {
 195             return 1;
 196         }
 197     }
 198
 199     return 0;
 200 }
 201
 202 static ZDLookupResult ZDPointInPolygon(ZoneDetect* library, uint32_t polygonIndex, int32_t latFixedPoint, int32_t lonFixedPoint, uint64_t* distanceSqrMin) {
 203     uint32_t numVertices;
 204     int32_t pointLat = 0, pointLon = 0, diffLat = 0, diffLon = 0, firstLat = 0, firstLon = 0, prevLat = 0, prevLon = 0;
 205     lonFixedPoint -= 3;
 206
 207     /* Read number of vertices */
 208     if(!ZDDecodeVariableLengthUnsigned(library, &polygonIndex, &numVertices)) return ZD_LOOKUP_PARSE_ERROR;
 209     if(numVertices > 1000000) return ZD_LOOKUP_PARSE_ERROR;
 210
 211     int prevQuadrant = 0, winding = 0;
 212
 213     uint32_t i;
 214     for(i=0; i<=numVertices; i++) {
 215         if(i<numVertices) {
 216             if(!ZDDecodeVariableLengthSigned(library, &polygonIndex, &diffLat)) return ZD_LOOKUP_PARSE_ERROR;
 217             if(!ZDDecodeVariableLengthSigned(library, &polygonIndex, &diffLon)) return ZD_LOOKUP_PARSE_ERROR;
 218             pointLat += diffLat;
 219             pointLon += diffLon;
 220             if(i==0) {
 221                 firstLat = pointLat;
 222                 firstLon = pointLon;
 223             }
 224         } else {
 225             /* The polygons should be closed, but just in case */
 226             pointLat = firstLat;
 227             pointLon = firstLon;
 228         }
 229
 230         /* Check if point is ON the border */
 231         if(pointLat == latFixedPoint && pointLon == lonFixedPoint) {
 232             if(distanceSqrMin) *distanceSqrMin=0;
 233             return ZD_LOOKUP_ON_BORDER_VERTEX;
 234         }
 235
 236         /* Find quadrant */
 237         int quadrant;
 238         if(pointLat>=latFixedPoint) {
 239             if(pointLon>=lonFixedPoint) {
 240                 quadrant = 0;
 241             } else {
 242                 quadrant = 1;
 243             }
 244         } else {
 245             if(pointLon>=lonFixedPoint) {
 246                 quadrant = 3;
 247             } else {
 248                 quadrant = 2;
 249             }
 250         }
 251
 252         if(i>0) {
 253             int windingNeedCompare = 0, lineIsStraight = 0;
 254             float a = 0, b = 0;
 255
 256             /* Calculate winding number */
 257             if(quadrant == prevQuadrant) {
 258                 /* Do nothing */
 259             } else if(quadrant == (prevQuadrant + 1) % 4) {
 260                 winding ++;
 261             } else if((quadrant + 1) % 4 == prevQuadrant) {
 262                 winding --;
 263             } else {
 264                 windingNeedCompare = 1;
 265             }
 266
 267             /* Avoid horizontal and vertical lines */
 268             if((pointLon == prevLon || pointLat == prevLat)) {
 269                 lineIsStraight = 1;
 270             }
 271
 272             /* Calculate the parameters of y=ax+b if needed */
 273             if(!lineIsStraight && (distanceSqrMin || windingNeedCompare)) {
 274                 a = ((float)pointLat - (float)prevLat)/((float)pointLon - prevLon);
 275                 b = (float)pointLat - a*(float)pointLon;
 276             }
 277
 278             /* Jumped two quadrants. */
 279             if(windingNeedCompare) {
 280                 if(lineIsStraight) {
 281                     if(distanceSqrMin) *distanceSqrMin=0;
 282                     return ZD_LOOKUP_ON_BORDER_SEGMENT;
 283                 }
 284
 285                 /* Check if the target is on the border */
 286                 int32_t intersectLon = ((float)latFixedPoint - b)/a;
 287                 if(intersectLon == lonFixedPoint) {
 288                     if(distanceSqrMin) *distanceSqrMin=0;
 289                     return ZD_LOOKUP_ON_BORDER_SEGMENT;
 290                 }
 291
 292                 /* Ok, it's not. In which direction did we go round the target? */
 293                 int sign = (intersectLon < lonFixedPoint)?2:-2;
 294                 if(quadrant == 2 || quadrant == 3) {
 295                     winding += sign;
 296                 } else {
 297                     winding -= sign;
 298                 }
 299             }
 300
 301             /* Calculate closest point on line (if needed) */
 302             if(distanceSqrMin) {
 303                 float closestLon, closestLat;
 304                 if(!lineIsStraight) {
 305                     closestLon=((float)lonFixedPoint+a*(float)latFixedPoint-a*b)/(a*a+1);
 306                     closestLat=(a*((float)lonFixedPoint+a*(float)latFixedPoint)+b)/(a*a+1);
 307                 } else {
 308                     if(pointLon == prevLon) {
 309                         closestLon=pointLon;
 310                         closestLat=latFixedPoint;
 311                     } else {
 312                         closestLon=lonFixedPoint;
 313                         closestLat=pointLat;
 314                     }
 315                 }
 316
 317                 int closestInBox = ZDPointInBox(pointLon, closestLon, prevLon, pointLat, closestLat, prevLat);
 318
 319                 int64_t diffLat, diffLon;
 320                 if(closestInBox) {
 321                     /* Calculate squared distance to segment. */
 322                     diffLat = closestLat - latFixedPoint;
 323                     diffLon = (closestLon - lonFixedPoint);
 324                 } else {
 325                     /*
 326                      * Calculate squared distance to vertices
 327                      * It is enough to check the current point since the polygon is closed.
 328                      */
 329                     diffLat = pointLat - latFixedPoint;
 330                     diffLon = (pointLon - lonFixedPoint);
 331                 }
 332
 333                 /* Note: lon has half scale */
 334                 uint64_t distanceSqr = diffLat*diffLat + diffLon*diffLon*4;
 335                 if(distanceSqr < *distanceSqrMin) *distanceSqrMin = distanceSqr;
 336             }
 337         }
 338
 339         prevQuadrant = quadrant;
 340         prevLat = pointLat;
 341         prevLon = pointLon;
 342     }
 343
 344     if(winding == -4) {
 345         return ZD_LOOKUP_IN_ZONE;
 346     } else if(winding == 4) {
 347         return ZD_LOOKUP_IN_EXCLUDED_ZONE;
 348     } else if(winding == 0) {
 349         return ZD_LOOKUP_NOT_IN_ZONE;
 350     }
 351
 352     /* Should not happen */
 353     if(distanceSqrMin) *distanceSqrMin=0;
 354     return ZD_LOOKUP_ON_BORDER_SEGMENT;
 355 }
 356
 357 void ZDCloseDatabase(ZoneDetect* library) {
 358     if(library) {
 359         if(library->fieldNames) {
 360             unsigned int i;
 361             for(i=0; i<library->numFields; i++) {
 362                 if(library->fieldNames[i]) {
 363                     free(library->fieldNames[i]);
 364                 }
 365             }
 366             free(library->fieldNames);
 367         }
 368         if(library->notice) {
 369             free(library->notice);
 370         }
 371         if(library->mapping) {
 372             munmap(library->mapping, library->length);
 373         }
 374         if(library->fd >= 0) {
 375             close(library->fd);
 376         }
 377         free(library);
 378     }
 379 }
 380
 381 ZoneDetect* ZDOpenDatabase(const char* path) {
 382     ZoneDetect* library = (ZoneDetect*)malloc(sizeof(*library));
 383
 384     if(library) {
 385         memset(library, 0, sizeof(*library));
 386
 387         library->fd = open(path, O_RDONLY | O_CLOEXEC);
 388         if(library->fd < 0) {
 389             goto fail;
 390         }
 391
 392         library->length = lseek(library->fd, 0, SEEK_END);
 393         if(library->length <= 0) {
 394             goto fail;
 395         }
 396         lseek(library->fd, 0, SEEK_SET);
 397
 398         library->mapping = mmap(NULL, library->length, PROT_READ, MAP_PRIVATE | MAP_FILE, library->fd, 0);
 399         if(!library->mapping) {
 400             goto fail;
 401         }
 402
 403         /* Parse the header */
 404         if(ZDParseHeader(library)) {
 405             goto fail;
 406         }
 407     }
 408
 409     return library;
 410
 411 fail:
 412     ZDCloseDatabase(library);
 413     return NULL;
 414 }
 415
 416 ZoneDetectResult* ZDLookup(ZoneDetect* library, float lat, float lon, float* safezone) {
 417     int32_t latFixedPoint = ZDFloatToFixedPoint(lat, 90, library->precision);
 418     int32_t lonFixedPoint = ZDFloatToFixedPoint(lon, 180, library->precision);
 419     unsigned int numResults = 0;
 420     uint64_t distanceSqrMin=-1;
 421
 422     /* Iterate over all polygons */
 423     uint32_t bboxIndex = library->bboxOffset;
 424     int32_t metadataIndex = 0;
 425     int32_t polygonIndex = 0;
 426
 427     ZoneDetectResult* results = malloc(sizeof(ZoneDetectResult));
 428     if(!results) {
 429         return NULL;
 430     }
 431
 432     while(bboxIndex < library->metadataOffset) {
 433         int32_t minLat, minLon, maxLat, maxLon, metadataIndexDelta;
 434         uint32_t polygonIndexDelta;
 435         if(!ZDDecodeVariableLengthSigned(library, &bboxIndex, &minLat)) break;
 436         if(!ZDDecodeVariableLengthSigned(library, &bboxIndex, &minLon)) break;
 437         if(!ZDDecodeVariableLengthSigned(library, &bboxIndex, &maxLat)) break;
 438         if(!ZDDecodeVariableLengthSigned(library, &bboxIndex, &maxLon)) break;
 439         if(!ZDDecodeVariableLengthSigned(library, &bboxIndex, &metadataIndexDelta)) break;
 440         if(!ZDDecodeVariableLengthUnsigned(library, &bboxIndex, &polygonIndexDelta)) break;
 441
 442         metadataIndex+=metadataIndexDelta;
 443         polygonIndex+=polygonIndexDelta;
 444
 445         if(latFixedPoint >= minLat) {
 446             if(latFixedPoint <= maxLat &&
 447                     lonFixedPoint >= minLon &&
 448                     lonFixedPoint <= maxLon) {
 449
 450                 /* Indices valid? */
 451                 if(library->metadataOffset + metadataIndex >= library->dataOffset) continue;
 452                 if(library->dataOffset + polygonIndex >= library->length) continue;
 453
 454                 ZDLookupResult lookupResult = ZDPointInPolygon(library, library->dataOffset + polygonIndex, latFixedPoint, lonFixedPoint, (safezone)?&distanceSqrMin:NULL);
 455                 if(lookupResult == ZD_LOOKUP_PARSE_ERROR) {
 456                     break;
 457                 } else if(lookupResult != ZD_LOOKUP_NOT_IN_ZONE) {
 458                     ZoneDetectResult* newResults = realloc(results, sizeof(ZoneDetectResult) * (numResults+2));
 459
 460                     if(newResults) {
 461                         results = newResults;
 462                         results[numResults].metaId = metadataIndex;
 463                         results[numResults].numFields = library->numFields;
 464                         results[numResults].fieldNames = library->fieldNames;
 465                         results[numResults].lookupResult = lookupResult;
 466
 467                         numResults++;
 468                     } else {
 469                         break;
 470                     }
 471                 }
 472             }
 473         } else {
 474             /* The data is sorted along minLat */
 475             break;
 476         }
 477     }
 478
 479     /* Clean up results */
 480     unsigned int i, j;
 481     for(i=0; i<numResults; i++) {
 482         int insideSum = 0;
 483         ZDLookupResult overrideResult = ZD_LOOKUP_IGNORE;
 484         for(j=i; j<numResults; j++) {
 485             if(results[i].metaId == results[j].metaId) {
 486                 ZDLookupResult tmpResult = results[j].lookupResult;
 487                 results[j].lookupResult = ZD_LOOKUP_IGNORE;
 488
 489                 /* This is the same result. Is it an exclusion zone? */
 490                 if(tmpResult == ZD_LOOKUP_IN_ZONE) {
 491                     insideSum++;
 492                 } else if(tmpResult == ZD_LOOKUP_IN_EXCLUDED_ZONE) {
 493                     insideSum--;
 494                 } else {
 495                     /* If on the bodrder then the final result is on the border */
 496                     overrideResult = tmpResult;
 497                 }
 498
 499             }
 500         }
 501
 502         if(overrideResult != ZD_LOOKUP_IGNORE) {
 503             results[i].lookupResult = overrideResult;
 504         } else {
 505             if(insideSum) {
 506                 results[i].lookupResult = ZD_LOOKUP_IN_ZONE;
 507             }
 508         }
 509     }
 510
 511     /* Remove zones to ignore */
 512     unsigned int newNumResults = 0;
 513     for(i=0; i<numResults; i++) {
 514         if(results[i].lookupResult != ZD_LOOKUP_IGNORE) {
 515             results[newNumResults] = results[i];
 516             newNumResults++;
 517         }
 518     }
 519     numResults = newNumResults;
 520
 521     /* Lookup metadata */
 522     for(i=0; i<numResults; i++) {
 523         uint32_t tmpIndex = library->metadataOffset + results[i].metaId;
 524         results[i].data = malloc(library->numFields * sizeof(char*));
 525         if(results[i].data) {
 526             for(j=0; j<library->numFields; j++) {
 527                 results[i].data[j] = ZDParseString(library, &tmpIndex);
 528             }
 529         }
 530     }
 531
 532     /* Write end marker */
 533     results[numResults].lookupResult = ZD_LOOKUP_END;
 534     results[numResults].numFields = 0;
 535     results[numResults].fieldNames = NULL;
 536     results[numResults].data = NULL;
 537
 538     if(safezone) {
 539         *safezone = sqrtf(distanceSqrMin) * 90 / (float)(1 << (library->precision-1));
 540     }
 541
 542     return results;
 543 }
 544
 545 void ZDFreeResults(ZoneDetectResult* results) {
 546     unsigned int index = 0;
 547
 548     if(!results) {
 549         return;
 550     }
 551
 552     while(results[index].lookupResult != ZD_LOOKUP_END) {
 553         if(results[index].data) {
 554             unsigned int i;
 555             for(i=0; i<results[index].numFields; i++) {
 556                 if(results[index].data[i]) {
 557                     free(results[index].data[i]);
 558                 }
 559             }
 560             free(results[index].data);
 561         }
 562         index++;
 563     }
 564     free(results);
 565 }
 566
 567 const char* ZDGetNotice(ZoneDetect* library) {
 568     return library->notice;
 569 }
 570
 571 uint8_t ZDGetTableType(ZoneDetect* library) {
 572     return library->tableType;
 573 }
 574
 575 const char* ZDLookupResultToString(ZDLookupResult result) {
 576     switch(result) {
 577     case ZD_LOOKUP_IGNORE:
 578         return "Ignore";
 579     case ZD_LOOKUP_END:
 580         return "End";
 581     case ZD_LOOKUP_PARSE_ERROR:
 582         return "Parsing error";
 583     case ZD_LOOKUP_NOT_IN_ZONE:
 584         return "Not in zone";
 585     case ZD_LOOKUP_IN_ZONE:
 586         return "In zone";
 587     case ZD_LOOKUP_IN_EXCLUDED_ZONE:
 588         return "In excluded zone";
 589     case ZD_LOOKUP_ON_BORDER_VERTEX:
 590         return "Target point is border vertex";
 591     case ZD_LOOKUP_ON_BORDER_SEGMENT:
 592         return "Target point is on border";
 593     }
 594
 595     return "Unknown";
 596 }
 597