603 lines
27 KiB
C++
603 lines
27 KiB
C++
/*
|
|
open source routing machine
|
|
Copyright (C) Dennis Luxen, others 2010
|
|
|
|
This program is free software; you can redistribute it and/or modify
|
|
it under the terms of the GNU AFFERO General Public License as published by
|
|
the Free Software Foundation; either version 3 of the License, or
|
|
any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU Affero General Public License
|
|
along with this program; if not, write to the Free Software
|
|
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
or see http://www.gnu.org/licenses/agpl.txt.
|
|
*/
|
|
|
|
#ifndef NNGRID_H_
|
|
#define NNGRID_H_
|
|
|
|
#include <cassert>
|
|
#include <cfloat>
|
|
#include <cmath>
|
|
#include <cstring>
|
|
|
|
#include <algorithm>
|
|
#include <fstream>
|
|
#include <limits>
|
|
#include <vector>
|
|
|
|
#ifndef ROUTED
|
|
#include <stxxl.h>
|
|
#endif
|
|
|
|
#ifdef _WIN32
|
|
#include <math.h>
|
|
#endif
|
|
|
|
#include <boost/thread.hpp>
|
|
#include <boost/foreach.hpp>
|
|
#include <boost/unordered_map.hpp>
|
|
|
|
#include "DeallocatingVector.h"
|
|
#include "GridEdge.h"
|
|
#include "Percent.h"
|
|
#include "PhantomNodes.h"
|
|
#include "MercatorUtil.h"
|
|
#include "StaticGraph.h"
|
|
#include "TimingUtil.h"
|
|
#include "../Algorithms/Bresenham.h"
|
|
|
|
namespace NNGrid{
|
|
|
|
static boost::thread_specific_ptr<std::ifstream> localStream;
|
|
|
|
template<bool WriteAccess = false>
|
|
class NNGrid {
|
|
public:
|
|
NNGrid() /*: cellCache(500), fileCache(500)*/ {
|
|
ramIndexTable.resize((1024*1024), std::numeric_limits<uint64_t>::max());
|
|
}
|
|
|
|
NNGrid(const char* rif, const char* _i) {
|
|
if(WriteAccess) {
|
|
ERR("Not available in Write mode");
|
|
}
|
|
iif = std::string(_i);
|
|
ramIndexTable.resize((1024*1024), std::numeric_limits<uint64_t>::max());
|
|
ramInFile.open(rif, std::ios::in | std::ios::binary);
|
|
if(!ramInFile) { ERR(rif << " not found"); }
|
|
|
|
}
|
|
|
|
~NNGrid() {
|
|
if(ramInFile.is_open()) ramInFile.close();
|
|
|
|
#ifndef ROUTED
|
|
if (WriteAccess) {
|
|
entries.clear();
|
|
}
|
|
#endif
|
|
if(localStream.get() && localStream->is_open()) {
|
|
localStream->close();
|
|
}
|
|
}
|
|
|
|
void OpenIndexFiles() {
|
|
assert(ramInFile.is_open());
|
|
ramInFile.read(static_cast<char*>(static_cast<void*>(&ramIndexTable[0]) ), sizeof(uint64_t)*1024*1024);
|
|
ramInFile.close();
|
|
}
|
|
|
|
#ifndef ROUTED
|
|
template<typename EdgeT>
|
|
inline void ConstructGrid(DeallocatingVector<EdgeT> & edgeList, const char * ramIndexOut, const char * fileIndexOut) {
|
|
//TODO: Implement this using STXXL-Streams
|
|
Percent p(edgeList.size());
|
|
BOOST_FOREACH(EdgeT & edge, edgeList) {
|
|
p.printIncrement();
|
|
if(edge.ignoreInGrid)
|
|
continue;
|
|
int slat = 100000*lat2y(edge.lat1/100000.);
|
|
int slon = edge.lon1;
|
|
int tlat = 100000*lat2y(edge.lat2/100000.);
|
|
int tlon = edge.lon2;
|
|
AddEdge( _GridEdge( edge.id, edge.nameID, edge.weight, _Coordinate(slat, slon), _Coordinate(tlat, tlon), edge.belongsToTinyComponent ) );
|
|
}
|
|
if( 0 == entries.size() ) {
|
|
ERR("No viable edges for nearest neighbor index. Aborting");
|
|
}
|
|
double timestamp = get_timestamp();
|
|
//create index file on disk, old one is over written
|
|
indexOutFile.open(fileIndexOut, std::ios::out | std::ios::binary | std::ios::trunc);
|
|
//sort entries
|
|
stxxl::sort(entries.begin(), entries.end(), CompareGridEdgeDataByRamIndex(), 1024*1024*1024);
|
|
INFO("finished sorting after " << (get_timestamp() - timestamp) << "s");
|
|
std::vector<GridEntry> entriesInFileWithRAMSameIndex;
|
|
unsigned indexInRamTable = entries.begin()->ramIndex;
|
|
uint64_t lastPositionInIndexFile = 0;
|
|
std::cout << "writing data ..." << std::flush;
|
|
p.reinit(entries.size());
|
|
boost::unordered_map< unsigned, unsigned > cellMap(1024);
|
|
BOOST_FOREACH(GridEntry & gridEntry, entries) {
|
|
p.printIncrement();
|
|
if(gridEntry.ramIndex != indexInRamTable) {
|
|
cellMap.clear();
|
|
BuildCellIndexToFileIndexMap(indexInRamTable, cellMap);
|
|
|
|
unsigned numberOfBytesInCell = FillCell(entriesInFileWithRAMSameIndex, lastPositionInIndexFile, cellMap);
|
|
ramIndexTable[indexInRamTable] = lastPositionInIndexFile;
|
|
lastPositionInIndexFile += numberOfBytesInCell;
|
|
entriesInFileWithRAMSameIndex.clear();
|
|
indexInRamTable = gridEntry.ramIndex;
|
|
}
|
|
entriesInFileWithRAMSameIndex.push_back(gridEntry);
|
|
}
|
|
cellMap.clear();
|
|
BuildCellIndexToFileIndexMap(indexInRamTable, cellMap);
|
|
/*unsigned numberOfBytesInCell = */FillCell(entriesInFileWithRAMSameIndex, lastPositionInIndexFile, cellMap);
|
|
ramIndexTable[indexInRamTable] = lastPositionInIndexFile;
|
|
entriesInFileWithRAMSameIndex.clear();
|
|
std::vector<GridEntry>().swap(entriesInFileWithRAMSameIndex);
|
|
assert(entriesInFileWithRAMSameIndex.size() == 0);
|
|
//close index file
|
|
indexOutFile.close();
|
|
|
|
//Serialize RAM Index
|
|
std::ofstream ramFile(ramIndexOut, std::ios::out | std::ios::binary | std::ios::trunc);
|
|
//write 4 MB of index Table in RAM
|
|
ramFile.write((char *)&ramIndexTable[0], sizeof(uint64_t)*1024*1024 );
|
|
//close ram index file
|
|
ramFile.close();
|
|
}
|
|
#endif
|
|
inline bool CoordinatesAreEquivalent(const _Coordinate & a, const _Coordinate & b, const _Coordinate & c, const _Coordinate & d) const {
|
|
return (a == b && c == d) || (a == c && b == d) || (a == d && b == c);
|
|
}
|
|
|
|
bool FindPhantomNodeForCoordinate( const _Coordinate & location, PhantomNode & resultNode, const unsigned zoomLevel) {
|
|
bool ignoreTinyComponents = (zoomLevel <= 14);
|
|
// INFO("Coordinate: " << location << ", zoomLevel: " << zoomLevel << ", ignoring tinyComponentents: " << (ignoreTinyComponents ? "yes" : "no"));
|
|
// double time1 = get_timestamp();
|
|
bool foundNode = false;
|
|
const _Coordinate startCoord(100000*(lat2y(static_cast<double>(location.lat)/100000.)), location.lon);
|
|
/** search for point on edge close to source */
|
|
const unsigned fileIndex = GetFileIndexForLatLon(startCoord.lat, startCoord.lon);
|
|
std::vector<_GridEdge> candidates;
|
|
const int lowerBoundForLoop = (fileIndex < 32768 ? 0 : -32768);
|
|
for(int j = lowerBoundForLoop; (j < (32768+1)) && (fileIndex != UINT_MAX); j+=32768) {
|
|
for(int i = -1; i < 2; ++i){
|
|
// unsigned oldSize = candidates.size();
|
|
GetContentsOfFileBucketEnumerated(fileIndex+i+j, candidates);
|
|
// INFO("Getting fileIndex=" << fileIndex+i+j << " with " << candidates.size() - oldSize << " candidates");
|
|
}
|
|
}
|
|
// INFO("looked up " << candidates.size());
|
|
_GridEdge smallestEdge;
|
|
_Coordinate tmp, edgeStartCoord, edgeEndCoord;
|
|
double dist = std::numeric_limits<double>::max();
|
|
double r, tmpDist;
|
|
|
|
BOOST_FOREACH(const _GridEdge & candidate, candidates) {
|
|
if(candidate.belongsToTinyComponent && ignoreTinyComponents)
|
|
continue;
|
|
r = 0.;
|
|
tmpDist = ComputeDistance(startCoord, candidate.startCoord, candidate.targetCoord, tmp, &r);
|
|
// INFO("dist " << startCoord << "->[" << candidate.startCoord << "-" << candidate.targetCoord << "]=" << tmpDist );
|
|
// INFO("Looking at edge " << candidate.edgeBasedNode << " at distance " << tmpDist);
|
|
if(tmpDist < dist && !DoubleEpsilonCompare(dist, tmpDist)) {
|
|
// INFO("a) " << candidate.edgeBasedNode << ", dist: " << tmpDist << ", tinyCC: " << (candidate.belongsToTinyComponent ? "yes" : "no"));
|
|
dist = tmpDist;
|
|
resultNode.edgeBasedNode = candidate.edgeBasedNode;
|
|
resultNode.nodeBasedEdgeNameID = candidate.nameID;
|
|
resultNode.weight1 = candidate.weight;
|
|
resultNode.weight2 = INT_MAX;
|
|
resultNode.location.lat = tmp.lat;
|
|
resultNode.location.lon = tmp.lon;
|
|
edgeStartCoord = candidate.startCoord;
|
|
edgeEndCoord = candidate.targetCoord;
|
|
foundNode = true;
|
|
smallestEdge = candidate;
|
|
//} else if(tmpDist < dist) {
|
|
//INFO("a) ignored " << candidate.edgeBasedNode << " at distance " << std::fabs(dist - tmpDist));
|
|
} else if(DoubleEpsilonCompare(dist, tmpDist) && 1 == std::abs(static_cast<int>(candidate.edgeBasedNode)-static_cast<int>(resultNode.edgeBasedNode) ) && CoordinatesAreEquivalent(edgeStartCoord, candidate.startCoord, edgeEndCoord, candidate.targetCoord)) {
|
|
resultNode.edgeBasedNode = std::min(candidate.edgeBasedNode, resultNode.edgeBasedNode);
|
|
resultNode.weight2 = candidate.weight;
|
|
//INFO("b) " << candidate.edgeBasedNode << ", dist: " << tmpDist);
|
|
}
|
|
}
|
|
|
|
// INFO("startcoord: " << smallestEdge.startCoord << ", tgtcoord" << smallestEdge.targetCoord << "result: " << newEndpoint);
|
|
// INFO("length of old edge: " << ApproximateDistance(smallestEdge.startCoord, smallestEdge.targetCoord));
|
|
// INFO("Length of new edge: " << ApproximateDistance(smallestEdge.startCoord, newEndpoint));
|
|
// assert(!resultNode.isBidirected() || (resultNode.weight1 == resultNode.weight2));
|
|
// if(resultNode.weight1 != resultNode.weight2) {
|
|
// INFO("-> Weight1: " << resultNode.weight1 << ", weight2: " << resultNode.weight2);
|
|
// INFO("-> node: " << resultNode.edgeBasedNode << ", bidir: " << (resultNode.isBidirected() ? "yes" : "no"));
|
|
// }
|
|
|
|
// INFO("startCoord: " << smallestEdge.startCoord << "; targetCoord: " << smallestEdge.targetCoord << "; newEndpoint: " << resultNode.location);
|
|
const double ratio = (foundNode ? std::min(1., ApproximateDistance(smallestEdge.startCoord, resultNode.location)/ApproximateDistance(smallestEdge.startCoord, smallestEdge.targetCoord)) : 0);
|
|
resultNode.location.lat = round(100000.*(y2lat(static_cast<double>(resultNode.location.lat)/100000.)));
|
|
// INFO("Length of vector: " << ApproximateDistance(smallestEdge.startCoord, resultNode.location)/ApproximateDistance(smallestEdge.startCoord, smallestEdge.targetCoord));
|
|
//Hack to fix rounding errors and wandering via nodes.
|
|
if(std::abs(location.lon - resultNode.location.lon) == 1)
|
|
resultNode.location.lon = location.lon;
|
|
if(std::abs(location.lat - resultNode.location.lat) == 1)
|
|
resultNode.location.lat = location.lat;
|
|
|
|
resultNode.weight1 *= ratio;
|
|
if(INT_MAX != resultNode.weight2) {
|
|
resultNode.weight2 *= (1.-ratio);
|
|
}
|
|
resultNode.ratio = ratio;
|
|
// INFO("start: " << edgeStartCoord << ", end: " << edgeEndCoord);
|
|
// INFO("selected node: " << resultNode.edgeBasedNode << ", bidirected: " << (resultNode.isBidirected() ? "yes" : "no"));
|
|
// INFO("New weight1: " << resultNode.weight1 << ", new weight2: " << resultNode.weight2 << ", ratio: " << ratio);
|
|
// INFO("distance to input coordinate: " << ApproximateDistance(location, resultNode.location) << "\n--");
|
|
// double time2 = get_timestamp();
|
|
// INFO("NN-Lookup in " << 1000*(time2-time1) << "ms");
|
|
return foundNode;
|
|
}
|
|
|
|
bool FindRoutingStarts(const _Coordinate& start, const _Coordinate& target, PhantomNodes & routingStarts, unsigned zoomLevel) {
|
|
routingStarts.Reset();
|
|
return (FindPhantomNodeForCoordinate( start, routingStarts.startPhantom, zoomLevel) &&
|
|
FindPhantomNodeForCoordinate( target, routingStarts.targetPhantom, zoomLevel) );
|
|
}
|
|
|
|
bool FindNearestCoordinateOnEdgeInNodeBasedGraph(const _Coordinate& inputCoordinate, _Coordinate& outputCoordinate, unsigned zoomLevel = 18) {
|
|
PhantomNode resultNode;
|
|
bool foundNode = FindPhantomNodeForCoordinate(inputCoordinate, resultNode, zoomLevel);
|
|
outputCoordinate = resultNode.location;
|
|
return foundNode;
|
|
}
|
|
|
|
void FindNearestPointOnEdge(const _Coordinate& inputCoordinate, _Coordinate& outputCoordinate) {
|
|
_Coordinate startCoord(100000*(lat2y(static_cast<double>(inputCoordinate.lat)/100000.)), inputCoordinate.lon);
|
|
unsigned fileIndex = GetFileIndexForLatLon(startCoord.lat, startCoord.lon);
|
|
|
|
std::vector<_GridEdge> candidates;
|
|
boost::unordered_map< unsigned, unsigned > cellMap;
|
|
for(int j = -32768; j < (32768+1); j+=32768) {
|
|
for(int i = -1; i < 2; ++i) {
|
|
GetContentsOfFileBucket(fileIndex+i+j, candidates, cellMap);
|
|
}
|
|
}
|
|
_Coordinate tmp;
|
|
double dist = (std::numeric_limits<double>::max)();
|
|
BOOST_FOREACH(const _GridEdge & candidate, candidates) {
|
|
double r = 0.;
|
|
double tmpDist = ComputeDistance(startCoord, candidate.startCoord, candidate.targetCoord, tmp, &r);
|
|
if(tmpDist < dist) {
|
|
dist = tmpDist;
|
|
outputCoordinate.lat = round(100000*(y2lat(static_cast<double>(tmp.lat)/100000.)));
|
|
outputCoordinate.lon = tmp.lon;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
private:
|
|
inline unsigned GetCellIndexFromRAMAndFileIndex(const unsigned ramIndex, const unsigned fileIndex) const {
|
|
unsigned lineBase = ramIndex/1024;
|
|
lineBase = lineBase*32*32768;
|
|
unsigned columnBase = ramIndex%1024;
|
|
columnBase=columnBase*32;
|
|
for (int i = 0;i < 32;++i) {
|
|
for (int j = 0;j < 32;++j) {
|
|
const unsigned localFileIndex = lineBase + i * 32768 + columnBase + j;
|
|
if(localFileIndex == fileIndex) {
|
|
unsigned cellIndex = i * 32 + j;
|
|
return cellIndex;
|
|
}
|
|
}
|
|
}
|
|
return UINT_MAX;
|
|
}
|
|
|
|
inline void BuildCellIndexToFileIndexMap(const unsigned ramIndex, boost::unordered_map<unsigned, unsigned >& cellMap){
|
|
unsigned lineBase = ramIndex/1024;
|
|
lineBase = lineBase*32*32768;
|
|
unsigned columnBase = ramIndex%1024;
|
|
columnBase=columnBase*32;
|
|
std::vector<std::pair<unsigned, unsigned> >insertionVector(1024);
|
|
for (int i = 0;i < 32;++i) {
|
|
for (int j = 0;j < 32;++j) {
|
|
unsigned fileIndex = lineBase + i * 32768 + columnBase + j;
|
|
unsigned cellIndex = i * 32 + j;
|
|
insertionVector[i * 32 + j] = std::make_pair(fileIndex, cellIndex);
|
|
}
|
|
}
|
|
cellMap.insert(insertionVector.begin(), insertionVector.end());
|
|
}
|
|
|
|
inline bool DoubleEpsilonCompare(const double d1, const double d2) const {
|
|
return (std::fabs(d1 - d2) < FLT_EPSILON);
|
|
}
|
|
|
|
#ifndef ROUTED
|
|
inline unsigned FillCell(std::vector<GridEntry>& entriesWithSameRAMIndex, const uint64_t fileOffset, boost::unordered_map< unsigned, unsigned > & cellMap ) {
|
|
std::vector<char> tmpBuffer(32*32*4096,0);
|
|
uint64_t indexIntoTmpBuffer = 0;
|
|
unsigned numberOfWrittenBytes = 0;
|
|
assert(indexOutFile.is_open());
|
|
|
|
std::vector<uint64_t> cellIndex(32*32,std::numeric_limits<uint64_t>::max());
|
|
|
|
for(unsigned i = 0; i < entriesWithSameRAMIndex.size() -1; ++i) {
|
|
assert(entriesWithSameRAMIndex[i].ramIndex== entriesWithSameRAMIndex[i+1].ramIndex);
|
|
}
|
|
|
|
//sort & unique
|
|
std::sort(entriesWithSameRAMIndex.begin(), entriesWithSameRAMIndex.end(), CompareGridEdgeDataByFileIndex());
|
|
// entriesWithSameRAMIndex.erase(std::unique(entriesWithSameRAMIndex.begin(), entriesWithSameRAMIndex.end()), entriesWithSameRAMIndex.end());
|
|
|
|
//traverse each file bucket and write its contents to disk
|
|
std::vector<GridEntry> entriesWithSameFileIndex;
|
|
unsigned fileIndex = entriesWithSameRAMIndex.begin()->fileIndex;
|
|
|
|
BOOST_FOREACH(GridEntry & gridEntry, entriesWithSameRAMIndex) {
|
|
assert(cellMap.find(gridEntry.fileIndex) != cellMap.end() ); //asserting that file index belongs to cell index
|
|
if(gridEntry.fileIndex != fileIndex) {
|
|
// start in cellIndex vermerken
|
|
int localFileIndex = entriesWithSameFileIndex.begin()->fileIndex;
|
|
int localCellIndex = cellMap.find(localFileIndex)->second;
|
|
assert(cellMap.find(entriesWithSameFileIndex.begin()->fileIndex) != cellMap.end());
|
|
|
|
cellIndex[localCellIndex] = indexIntoTmpBuffer + fileOffset;
|
|
indexIntoTmpBuffer += FlushEntriesWithSameFileIndexToBuffer(entriesWithSameFileIndex, tmpBuffer, indexIntoTmpBuffer);
|
|
fileIndex = gridEntry.fileIndex;
|
|
}
|
|
entriesWithSameFileIndex.push_back(gridEntry);
|
|
}
|
|
assert(cellMap.find(entriesWithSameFileIndex.begin()->fileIndex) != cellMap.end());
|
|
int localFileIndex = entriesWithSameFileIndex.begin()->fileIndex;
|
|
int localCellIndex = cellMap.find(localFileIndex)->second;
|
|
|
|
cellIndex[localCellIndex] = indexIntoTmpBuffer + fileOffset;
|
|
indexIntoTmpBuffer += FlushEntriesWithSameFileIndexToBuffer(entriesWithSameFileIndex, tmpBuffer, indexIntoTmpBuffer);
|
|
|
|
assert(entriesWithSameFileIndex.size() == 0);
|
|
indexOutFile.write(static_cast<char*>(static_cast<void*>(&cellIndex[0])),32*32*sizeof(uint64_t));
|
|
numberOfWrittenBytes += 32*32*sizeof(uint64_t);
|
|
|
|
//write contents of tmpbuffer to disk
|
|
indexOutFile.write(&tmpBuffer[0], indexIntoTmpBuffer*sizeof(char));
|
|
numberOfWrittenBytes += indexIntoTmpBuffer*sizeof(char);
|
|
|
|
return numberOfWrittenBytes;
|
|
}
|
|
|
|
inline unsigned FlushEntriesWithSameFileIndexToBuffer( std::vector<GridEntry> &vectorWithSameFileIndex, std::vector<char> & tmpBuffer, const uint64_t index) const {
|
|
sort( vectorWithSameFileIndex.begin(), vectorWithSameFileIndex.end() );
|
|
vectorWithSameFileIndex.erase(unique(vectorWithSameFileIndex.begin(), vectorWithSameFileIndex.end()), vectorWithSameFileIndex.end());
|
|
const unsigned lengthOfBucket = vectorWithSameFileIndex.size();
|
|
tmpBuffer.resize(tmpBuffer.size()+(sizeof(_GridEdge)*lengthOfBucket) + sizeof(unsigned) );
|
|
unsigned counter = 0;
|
|
|
|
for(unsigned i = 0; i < vectorWithSameFileIndex.size()-1; ++i) {
|
|
assert( vectorWithSameFileIndex[i].fileIndex == vectorWithSameFileIndex[i+1].fileIndex );
|
|
assert( vectorWithSameFileIndex[i].ramIndex == vectorWithSameFileIndex[i+1].ramIndex );
|
|
}
|
|
|
|
//write length of bucket
|
|
memcpy((char*)&(tmpBuffer[index+counter]), (char*)&lengthOfBucket, sizeof(lengthOfBucket));
|
|
counter += sizeof(lengthOfBucket);
|
|
|
|
BOOST_FOREACH(const GridEntry & entry, vectorWithSameFileIndex) {
|
|
char * data = (char*)&(entry.edge);
|
|
memcpy(static_cast<char*>(static_cast<void*>(&(tmpBuffer[index+counter]) )), data, sizeof(entry.edge));
|
|
counter += sizeof(entry.edge);
|
|
}
|
|
//Freeing data
|
|
vectorWithSameFileIndex.clear();
|
|
return counter;
|
|
}
|
|
#endif
|
|
|
|
inline void GetContentsOfFileBucketEnumerated(const unsigned fileIndex, std::vector<_GridEdge>& result) const {
|
|
unsigned ramIndex = GetRAMIndexFromFileIndex(fileIndex);
|
|
uint64_t startIndexInFile = ramIndexTable[ramIndex];
|
|
if(startIndexInFile == std::numeric_limits<uint64_t>::max()) {
|
|
return;
|
|
}
|
|
unsigned enumeratedIndex = GetCellIndexFromRAMAndFileIndex(ramIndex, fileIndex);
|
|
|
|
if(!localStream.get() || !localStream->is_open()) {
|
|
localStream.reset(new std::ifstream(iif.c_str(), std::ios::in | std::ios::binary));
|
|
}
|
|
if(!localStream->good()) {
|
|
localStream->clear(std::ios::goodbit);
|
|
DEBUG("Resetting stale filestream");
|
|
}
|
|
|
|
//only read the single necessary cell index
|
|
localStream->seekg(startIndexInFile+(enumeratedIndex*sizeof(uint64_t)));
|
|
uint64_t fetchedIndex = 0;
|
|
localStream->read(static_cast<char*>( static_cast<void*>(&fetchedIndex)), sizeof(uint64_t));
|
|
|
|
if(fetchedIndex == std::numeric_limits<uint64_t>::max()) {
|
|
return;
|
|
}
|
|
const uint64_t position = fetchedIndex + 32*32*sizeof(uint64_t) ;
|
|
|
|
unsigned lengthOfBucket;
|
|
unsigned currentSizeOfResult = result.size();
|
|
localStream->seekg(position);
|
|
localStream->read(static_cast<char*>( static_cast<void*>(&(lengthOfBucket))), sizeof(unsigned));
|
|
result.resize(currentSizeOfResult+lengthOfBucket);
|
|
localStream->read(static_cast<char*>( static_cast<void*>(&result[currentSizeOfResult])), lengthOfBucket*sizeof(_GridEdge));
|
|
}
|
|
|
|
inline void GetContentsOfFileBucket(const unsigned fileIndex, std::vector<_GridEdge>& result, boost::unordered_map< unsigned, unsigned> & cellMap) {
|
|
unsigned ramIndex = GetRAMIndexFromFileIndex(fileIndex);
|
|
uint64_t startIndexInFile = ramIndexTable[ramIndex];
|
|
if(startIndexInFile == std::numeric_limits<uint64_t>::max()) {
|
|
return;
|
|
}
|
|
|
|
uint64_t cellIndex[32*32];
|
|
|
|
cellMap.clear();
|
|
BuildCellIndexToFileIndexMap(ramIndex, cellMap);
|
|
if(!localStream.get() || !localStream->is_open()) {
|
|
localStream.reset(new std::ifstream(iif.c_str(), std::ios::in | std::ios::binary));
|
|
}
|
|
if(!localStream->good()) {
|
|
localStream->clear(std::ios::goodbit);
|
|
DEBUG("Resetting stale filestream");
|
|
}
|
|
|
|
localStream->seekg(startIndexInFile);
|
|
localStream->read(static_cast<char*>(static_cast<void*>( cellIndex)), 32*32*sizeof(uint64_t));
|
|
assert(cellMap.find(fileIndex) != cellMap.end());
|
|
if(cellIndex[cellMap[fileIndex]] == std::numeric_limits<uint64_t>::max()) {
|
|
return;
|
|
}
|
|
const uint64_t position = cellIndex[cellMap[fileIndex]] + 32*32*sizeof(uint64_t) ;
|
|
|
|
unsigned lengthOfBucket;
|
|
unsigned currentSizeOfResult = result.size();
|
|
localStream->seekg(position);
|
|
localStream->read(static_cast<char*>(static_cast<void*>(&(lengthOfBucket))), sizeof(unsigned));
|
|
result.resize(currentSizeOfResult+lengthOfBucket);
|
|
localStream->read(static_cast<char*>(static_cast<void*>(&result[currentSizeOfResult])), lengthOfBucket*sizeof(_GridEdge));
|
|
}
|
|
|
|
#ifndef ROUTED
|
|
inline void AddEdge(const _GridEdge & edge) {
|
|
std::vector<BresenhamPixel> indexList;
|
|
GetListOfIndexesForEdgeAndGridSize(edge.startCoord, edge.targetCoord, indexList);
|
|
for(unsigned i = 0; i < indexList.size(); ++i) {
|
|
entries.push_back(GridEntry(edge, indexList[i].first, indexList[i].second));
|
|
}
|
|
}
|
|
#endif
|
|
|
|
inline double ComputeDistance(const _Coordinate& inputPoint, const _Coordinate& source, const _Coordinate& target, _Coordinate& nearest, double *r) {
|
|
// INFO("comparing point " << inputPoint << " to edge [" << source << "-" << target << "]");
|
|
const double x = static_cast<double>(inputPoint.lat);
|
|
const double y = static_cast<double>(inputPoint.lon);
|
|
const double a = static_cast<double>(source.lat);
|
|
const double b = static_cast<double>(source.lon);
|
|
const double c = static_cast<double>(target.lat);
|
|
const double d = static_cast<double>(target.lon);
|
|
double p,q,mX,nY;
|
|
// INFO("x=" << x << ", y=" << y << ", a=" << a << ", b=" << b << ", c=" << c << ", d=" << d);
|
|
if(fabs(a-c) > FLT_EPSILON){
|
|
const double m = (d-b)/(c-a); // slope
|
|
// Projection of (x,y) on line joining (a,b) and (c,d)
|
|
p = ((x + (m*y)) + (m*m*a - m*b))/(1. + m*m);
|
|
q = b + m*(p - a);
|
|
}
|
|
else{
|
|
p = c;
|
|
q = y;
|
|
}
|
|
nY = (d*p - c*q)/(a*d - b*c);
|
|
mX = (p - nY*a)/c;// These values are actually n/m+n and m/m+n , we neednot calculate the values of m an n as we are just interested in the ratio
|
|
// INFO("p=" << p << ", q=" << q << ", nY=" << nY << ", mX=" << mX);
|
|
if(std::isnan(mX)) {
|
|
*r = (target == inputPoint) ? 1. : 0.;
|
|
} else {
|
|
*r = mX;
|
|
}
|
|
// INFO("r=" << *r);
|
|
if(*r<=0.){
|
|
nearest.lat = source.lat;
|
|
nearest.lon = source.lon;
|
|
// INFO("a returning distance " << ((b - y)*(b - y) + (a - x)*(a - x)))
|
|
return ((b - y)*(b - y) + (a - x)*(a - x));
|
|
}
|
|
else if(*r >= 1.){
|
|
nearest.lat = target.lat;
|
|
nearest.lon = target.lon;
|
|
// INFO("b returning distance " << ((d - y)*(d - y) + (c - x)*(c - x)))
|
|
|
|
return ((d - y)*(d - y) + (c - x)*(c - x));
|
|
}
|
|
// point lies in between
|
|
nearest.lat = p;
|
|
nearest.lon = q;
|
|
// INFO("c returning distance " << (p-x)*(p-x) + (q-y)*(q-y))
|
|
|
|
return (p-x)*(p-x) + (q-y)*(q-y);
|
|
}
|
|
|
|
inline void GetListOfIndexesForEdgeAndGridSize(const _Coordinate& start, const _Coordinate& target, std::vector<BresenhamPixel> &indexList) const {
|
|
double lat1 = start.lat/100000.;
|
|
double lon1 = start.lon/100000.;
|
|
|
|
double x1 = ( lon1 + 180.0 ) / 360.0;
|
|
double y1 = ( lat1 + 180.0 ) / 360.0;
|
|
|
|
double lat2 = target.lat/100000.;
|
|
double lon2 = target.lon/100000.;
|
|
|
|
double x2 = ( lon2 + 180.0 ) / 360.0;
|
|
double y2 = ( lat2 + 180.0 ) / 360.0;
|
|
|
|
Bresenham(x1*32768, y1*32768, x2*32768, y2*32768, indexList);
|
|
BOOST_FOREACH(BresenhamPixel & pixel, indexList) {
|
|
int fileIndex = (pixel.second-1)*32768 + pixel.first;
|
|
int ramIndex = GetRAMIndexFromFileIndex(fileIndex);
|
|
pixel.first = fileIndex;
|
|
pixel.second = ramIndex;
|
|
}
|
|
}
|
|
|
|
inline unsigned GetFileIndexForLatLon(const int lt, const int ln) const {
|
|
double lat = lt/100000.;
|
|
double lon = ln/100000.;
|
|
|
|
double x = ( lon + 180.0 ) / 360.0;
|
|
double y = ( lat + 180.0 ) / 360.0;
|
|
|
|
if( x>1.0 || x < 0.)
|
|
return UINT_MAX;
|
|
if( y>1.0 || y < 0.)
|
|
return UINT_MAX;
|
|
|
|
unsigned line = (32768 * (32768-1))*y;
|
|
line = line - (line % 32768);
|
|
assert(line % 32768 == 0);
|
|
unsigned column = 32768.*x;
|
|
unsigned fileIndex = line+column;
|
|
return fileIndex;
|
|
}
|
|
|
|
inline unsigned GetRAMIndexFromFileIndex(const int fileIndex) const {
|
|
unsigned fileLine = fileIndex / 32768;
|
|
fileLine = fileLine / 32;
|
|
fileLine = fileLine * 1024;
|
|
unsigned fileColumn = (fileIndex % 32768);
|
|
fileColumn = fileColumn / 32;
|
|
unsigned ramIndex = fileLine + fileColumn;
|
|
assert(ramIndex < 1024*1024);
|
|
return ramIndex;
|
|
}
|
|
|
|
const static uint64_t END_OF_BUCKET_DELIMITER = boost::integer_traits<uint64_t>::const_max;
|
|
|
|
std::ifstream ramInFile;
|
|
#ifndef ROUTED
|
|
std::ofstream indexOutFile;
|
|
stxxl::vector<GridEntry> entries;
|
|
#endif
|
|
std::vector<uint64_t> ramIndexTable; //8 MB for first level index in RAM
|
|
std::string iif;
|
|
// LRUCache<int,std::vector<unsigned> > cellCache;
|
|
// LRUCache<int,std::vector<_Edge> > fileCache;
|
|
};
|
|
}
|
|
|
|
typedef NNGrid::NNGrid<false> ReadOnlyGrid;
|
|
typedef NNGrid::NNGrid<true > WritableGrid;
|
|
|
|
#endif /* NNGRID_H_ */
|