bool TestImporter::GetBoundingBox( BoundingBox* box )
{
FileStream boundingBoxData( fileInDirectory( m_outputDirectory, "Test Importer" ) + "_bounding_box" );
if ( !boundingBoxData.open( QIODevice::ReadOnly ) )
return false;
GPSCoordinate minGPS, maxGPS;
boundingBoxData >> minGPS.latitude >> minGPS.longitude >> maxGPS.latitude >> maxGPS.longitude;
if ( boundingBoxData.status() == QDataStream::ReadPastEnd ) {
qCritical() << "error reading bounding box";
return false;
}
box->min = UnsignedCoordinate( minGPS );
box->max = UnsignedCoordinate( maxGPS );
if ( box->min.x > box->max.x )
std::swap( box->min.x, box->max.x );
if ( box->min.y > box->max.y )
std::swap( box->min.y, box->max.y );
return true;
}
bool Logger::readGpxLog()
{
m_gpsInfoBuffer.clear();
if ( !m_logFile.open( QIODevice::ReadOnly | QIODevice::Text ) )
{
return false;
}
QString lineBuffer;
QString latString;
QString lonString;
QString eleString;
QString timeString;
QStringList tempList;
bool insideTrackpoint = false;
while ( !m_logFile.atEnd() )
{
lineBuffer = m_logFile.readLine();
lineBuffer = lineBuffer.simplified();
if (!insideTrackpoint)
{
latString = "";
lonString = "";
eleString = "";
timeString = "";
}
if (lineBuffer.contains("<trkpt"))
{
insideTrackpoint = true;
tempList = lineBuffer.split("\"");
latString = tempList.at(1);
lonString = tempList.at(3);
}
if (lineBuffer.contains("<ele>"))
{
lineBuffer = lineBuffer.remove("<ele>");
lineBuffer = lineBuffer.remove("</ele>");
eleString = lineBuffer;
}
if (lineBuffer.contains("<time>"))
{
lineBuffer = lineBuffer.remove("<time>");
lineBuffer = lineBuffer.remove("</time>");
timeString = lineBuffer;
}
if (lineBuffer.contains("</trkpt>"))
{
RoutingLogic::GPSInfo gpsInfo;
gpsInfo.position = UnsignedCoordinate( GPSCoordinate() );
gpsInfo.altitude = -1;
gpsInfo.groundSpeed = -1;
gpsInfo.verticalSpeed = -1;
gpsInfo.heading = -1;
gpsInfo.horizontalAccuracy = -1;
gpsInfo.verticalAccuracy = -1;
QDateTime invalidTime;
gpsInfo.timestamp = invalidTime;
gpsInfo.position = UnsignedCoordinate( GPSCoordinate( latString.toDouble(), lonString.toDouble() ) );
gpsInfo.altitude = eleString.toDouble();
gpsInfo.timestamp = QDateTime::fromString( timeString, "yyyy-MM-ddTHH:mm:ss" );
m_gpsInfoBuffer.append(gpsInfo);
}
if (lineBuffer.contains("</trkseg>"))
{
RoutingLogic::GPSInfo gpsInfo;
gpsInfo.position = UnsignedCoordinate( GPSCoordinate() );
gpsInfo.altitude = -1;
gpsInfo.groundSpeed = -1;
gpsInfo.verticalSpeed = -1;
gpsInfo.heading = -1;
gpsInfo.horizontalAccuracy = -1;
gpsInfo.verticalAccuracy = -1;
QDateTime invalidTime;
gpsInfo.timestamp = invalidTime;
m_gpsInfoBuffer.append(gpsInfo);
}
}
m_logFile.close();
emit trackChanged();
return true;
}
bool GPSGridClient::GetNearestEdge( Result* result, const UnsignedCoordinate& coordinate, double radius, double headingPenalty, double heading )
{
const GPSCoordinate gps = coordinate.ToProjectedCoordinate().ToGPSCoordinate();
const GPSCoordinate gpsMoved( gps.latitude, gps.longitude + 1 );
const double unsigned_per_meter = (( double ) UnsignedCoordinate( ProjectedCoordinate( gpsMoved ) ).x - coordinate.x ) / gps.ApproximateDistance( gpsMoved );
// Convert radius and headingPenalty from meters to unsigned^2.
double gridRadius = unsigned_per_meter * radius;
double gridRadius2 = gridRadius * gridRadius;
double gridHeadingPenalty = unsigned_per_meter * headingPenalty;
double gridHeadingPenalty2 = gridHeadingPenalty * gridHeadingPenalty;
// Convert heading from 'degrees from North' to 'radians from x-axis'
// (clockwise, [0, 0] is topleft corner, [1, 1] is bottomright corner).
heading = fmod( ( heading + 270 ) * 2.0 * M_PI / 360.0, 2 * M_PI );
static const int width = 32 * 32 * 32;
ProjectedCoordinate position = coordinate.ToProjectedCoordinate();
NodeID yGrid = floor( position.y * width );
NodeID xGrid = floor( position.x * width );
// Set the distance to the nearest edge initially to infinity.
result->gridDistance2 = 1e20;
QVector< UnsignedCoordinate > path;
checkCell( result, &path, xGrid - 1, yGrid - 1, coordinate, gridRadius2, gridHeadingPenalty2, heading );
checkCell( result, &path, xGrid - 1, yGrid, coordinate, gridRadius2, gridHeadingPenalty2, heading );
checkCell( result, &path, xGrid - 1, yGrid + 1, coordinate, gridRadius2, gridHeadingPenalty2, heading );
checkCell( result, &path, xGrid, yGrid - 1, coordinate, gridRadius2, gridHeadingPenalty2, heading );
checkCell( result, &path, xGrid, yGrid, coordinate, gridRadius2, gridHeadingPenalty2, heading );
checkCell( result, &path, xGrid, yGrid + 1, coordinate, gridRadius2, gridHeadingPenalty2, heading );
checkCell( result, &path, xGrid + 1, yGrid - 1, coordinate, gridRadius2, gridHeadingPenalty2, heading );
checkCell( result, &path, xGrid + 1, yGrid, coordinate, gridRadius2, gridHeadingPenalty2, heading );
checkCell( result, &path, xGrid + 1, yGrid + 1, coordinate, gridRadius2, gridHeadingPenalty2, heading );
if ( path.empty() )
return false;
double length = 0;
double lengthToNearest = 0;
for ( int pathID = 1; pathID < path.size(); pathID++ ) {
UnsignedCoordinate sourceCoord = path[pathID - 1];
UnsignedCoordinate targetCoord = path[pathID];
double xDiff = ( double ) sourceCoord.x - targetCoord.x;
double yDiff = ( double ) sourceCoord.y - targetCoord.y;
double distance = sqrt( xDiff * xDiff + yDiff * yDiff );
length += distance;
if ( pathID < ( int ) result->previousWayCoordinates )
lengthToNearest += distance;
else if ( pathID == ( int ) result->previousWayCoordinates )
lengthToNearest += result->percentage * distance;
}
if ( length == 0 )
result->percentage = 0;
else
result->percentage = lengthToNearest / length;
return true;
}
bool GPSGrid::Preprocess( IImporter* importer, QString dir )
{
QString filename = fileInDirectory( dir, "GPSGrid" );
QFile gridFile( filename + "_grid" );
QFile configFile( filename + "_config" );
if ( !openQFile( &gridFile, QIODevice::WriteOnly ) )
return false;
if ( !openQFile( &configFile, QIODevice::WriteOnly ) )
return false;
std::vector< IImporter::RoutingNode > inputNodes;
std::vector< IImporter::RoutingEdge > inputEdges;
std::vector< unsigned > nodeIDs;
std::vector< unsigned > edgeIDs;
std::vector< IImporter::RoutingNode > edgePaths;
if ( !importer->GetRoutingNodes( &inputNodes ) )
return false;
if ( !importer->GetRoutingEdges( &inputEdges ) )
return false;
if ( !importer->GetIDMap( &nodeIDs ) )
return false;
if ( !importer->GetEdgeIDMap( &edgeIDs ) )
return false;
if ( !importer->GetRoutingEdgePaths( &edgePaths ) )
return false;
static const int width = 32 * 32 * 32;
std::vector< GridImportEdge > grid;
Timer time;
for ( std::vector< IImporter::RoutingEdge >::const_iterator i = inputEdges.begin(); i != inputEdges.end(); ++i ) {
std::vector< UnsignedCoordinate > path;
path.push_back( inputNodes[i->source].coordinate );
for ( unsigned pathID = 0; pathID < i->pathLength; pathID++ )
path.push_back( edgePaths[pathID + i->pathID].coordinate );
path.push_back( inputNodes[i->target].coordinate );
std::vector< std::pair< unsigned, unsigned > > gridCells;
for ( unsigned segment = 1; segment < path.size(); segment++ ) {
ProjectedCoordinate sourceCoordinate = path[segment - 1].ToProjectedCoordinate();
ProjectedCoordinate targetCoordinate = path[segment].ToProjectedCoordinate();
sourceCoordinate.x *= width;
sourceCoordinate.y *= width;
targetCoordinate.x *= width;
targetCoordinate.y *= width;
NodeID minYGrid = floor( sourceCoordinate.y );
NodeID minXGrid = floor( sourceCoordinate.x );
NodeID maxYGrid = floor( targetCoordinate.y );
NodeID maxXGrid = floor( targetCoordinate.x );
if ( minYGrid > maxYGrid )
std::swap( minYGrid, maxYGrid );
if ( minXGrid > maxXGrid )
std::swap( minXGrid, maxXGrid );
for ( NodeID yGrid = minYGrid; yGrid <= maxYGrid; ++yGrid ) {
for ( NodeID xGrid = minXGrid; xGrid <= maxXGrid; ++xGrid ) {
if ( !clipEdge( sourceCoordinate, targetCoordinate, ProjectedCoordinate( xGrid, yGrid ), ProjectedCoordinate( xGrid + 1, yGrid + 1 ) ) )
continue;
gridCells.push_back( std::pair< unsigned, unsigned >( xGrid, yGrid ) );
}
}
}
std::sort( gridCells.begin(), gridCells.end() );
gridCells.resize( std::unique( gridCells.begin(), gridCells.end() ) - gridCells.begin() );
GridImportEdge clippedEdge;
clippedEdge.edge = i - inputEdges.begin();
for ( unsigned cell = 0; cell < gridCells.size(); cell++ ) {
clippedEdge.x = gridCells[cell].first;
clippedEdge.y = gridCells[cell].second;
grid.push_back( clippedEdge );
}
}
qDebug() << "GPS Grid: distributed edges:" << time.restart() << "ms";
qDebug() << "GPS Grid: overhead:" << grid.size() - inputEdges.size() << "duplicated edges";
qDebug() << "GPS Grid: overhead:" << ( grid.size() - inputEdges.size() ) * 100 / inputEdges.size() << "% duplicated edges";
std::sort( grid.begin(), grid.end() );
qDebug() << "GPS Grid: sorted edges:" << time.restart() << "ms";
std::vector< gg::GridIndex > tempIndex;
qint64 position = 0;
for ( std::vector< GridImportEdge >::const_iterator edge = grid.begin(); edge != grid.end(); ) {
gg::Cell cell;
gg::GridIndex entry;
entry.x = edge->x;
entry.y = edge->y;
entry.position = position;
tempIndex.push_back( entry );
do {
const IImporter::RoutingEdge& originalEdge = inputEdges[edge->edge];
gg::Cell::Edge newEdge;
newEdge.source = nodeIDs[originalEdge.source];
newEdge.target = nodeIDs[originalEdge.target];
newEdge.edgeID = edgeIDs[edge->edge];
newEdge.bidirectional = originalEdge.bidirectional;
newEdge.pathID = cell.coordinates.size();
newEdge.pathLength = 2 + originalEdge.pathLength;
cell.coordinates.push_back( inputNodes[originalEdge.source].coordinate );
for ( unsigned pathID = 0; pathID < originalEdge.pathLength; pathID++ )
cell.coordinates.push_back( edgePaths[pathID + originalEdge.pathID].coordinate );
cell.coordinates.push_back( inputNodes[originalEdge.target].coordinate );
cell.edges.push_back( newEdge );
edge++;
} while ( edge != grid.end() && edge->x == entry.x && edge->y == entry.y );
ProjectedCoordinate min( ( double ) entry.x / width, ( double ) entry.y / width );
ProjectedCoordinate max( ( double ) ( entry.x + 1 ) / width, ( double ) ( entry.y + 1 ) / width );
unsigned maxSize = cell.edges.size() * sizeof( gg::Cell::Edge ) * 2 + cell.coordinates.size() * sizeof( UnsignedCoordinate ) * 2 + 100;
unsigned char* buffer = new unsigned char[maxSize];
memset( buffer, 0, maxSize );
int size = cell.write( buffer, UnsignedCoordinate( min ), UnsignedCoordinate( max ) );
assert( size < ( int ) maxSize );
#ifndef NDEBUG
gg::Cell unpackCell;
unpackCell.read( buffer, UnsignedCoordinate( min ), UnsignedCoordinate( max ) );
assert( unpackCell == cell );
#endif
gridFile.write( ( const char* ) &size, sizeof( size ) );
gridFile.write( ( const char* ) buffer, size );
delete[] buffer;
position += size + sizeof( size );
}
qDebug() << "GPS Grid: wrote cells:" << time.restart() << "ms";
gg::Index::Create( filename + "_index", tempIndex );
qDebug() << "GPS Grid: created index:" << time.restart() << "ms";
return true;
}
