static bool clipTriangleAgainstNearplane(const mat<4,3,float>& triangleIn, b3AlignedObjectArray<mat<4,3,float> >& clippedTrianglesOut)
{
//discard triangle if all vertices are behind near-plane
if (triangleIn[3][0]<0 && triangleIn[3][1] <0 && triangleIn[3][2] <0)
{
return true;
}
//accept triangle if all vertices are in front of the near-plane
if (triangleIn[3][0]>=0 && triangleIn[3][1] >=0 && triangleIn[3][2] >=0)
{
clippedTrianglesOut.push_back(triangleIn);
return false;
}
Vec4f vtxCache[5];
b3AlignedObjectArray<Vec4f> vertices;
vertices.initializeFromBuffer(vtxCache,0,5);
clipEdge(triangleIn,0,1,vertices);
clipEdge(triangleIn,1,2,vertices);
clipEdge(triangleIn,2,0,vertices);
if (vertices.size()<3)
return true;
if (equals(vertices[0],vertices[vertices.size()-1]))
{
vertices.pop_back();
}
//create a fan of triangles
for (int i=1;i<vertices.size()-1;i++)
{
mat<4,3,float>& vtx = clippedTrianglesOut.expand();
vtx.set_col(0,vertices[0]);
vtx.set_col(1,vertices[i]);
vtx.set_col(2,vertices[i+1]);
}
return true;
}
QwtPointArray QwtRect::clip(const QwtPointArray &pa) const
{
if ( contains( pa.boundingRect() ) )
return pa;
QwtPointArray cpa(pa.size());
clipEdge((Edge)0, pa, cpa);
for ( uint edge = 1; edge < NEdges; edge++ )
{
const QwtPointArray rpa = cpa;
#if QT_VERSION < 0x040000
cpa.detach();
#endif
clipEdge((Edge)edge, rpa, cpa);
}
return cpa;
}
//! Sutherland-Hodgman polygon clipping
QwtPolygonF QwtPolygonClipperF::clipPolygon(const QwtPolygonF &pa) const
{
if ( contains( ::boundingRect(pa) ) )
return pa;
QwtPolygonF cpa(pa.size());
clipEdge((Edge)0, pa, cpa);
for ( uint edge = 1; edge < NEdges; edge++ )
{
const QwtPolygonF rpa = cpa;
#if QT_VERSION < 0x040000
cpa.detach();
#endif
clipEdge((Edge)edge, rpa, cpa);
}
return cpa;
}
void Clipper::clipFar(Polygon &polygon)
{
const float4 **V = polygon.P[polygon.i];
const float4 **T = polygon.P[polygon.i + 1];
int t = 0;
for(int i = 0; i < polygon.n; i++)
{
int j = i == polygon.n - 1 ? 0 : i + 1;
float di = V[i]->w - V[i]->z;
float dj = V[j]->w - V[j]->z;
if(di >= 0)
{
T[t++] = V[i];
if(dj < 0)
{
clipEdge(polygon.B[polygon.b], *V[i], *V[j], di, dj);
T[t++] = &polygon.B[polygon.b++];
}
}
else
{
if(dj > 0)
{
clipEdge(polygon.B[polygon.b], *V[j], *V[i], dj, di);
T[t++] = &polygon.B[polygon.b++];
}
}
}
polygon.n = t;
polygon.i += 1;
}
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;
}
