Root/
| 1 | /* |
| 2 | Copyright 2010 Christian Vetter veaac.fdirct@gmail.com |
| 3 | |
| 4 | This file is part of MoNav. |
| 5 | |
| 6 | MoNav is free software: you can redistribute it and/or modify |
| 7 | it under the terms of the GNU General Public License as published by |
| 8 | the Free Software Foundation, either version 3 of the License, or |
| 9 | (at your option) any later version. |
| 10 | |
| 11 | MoNav is distributed in the hope that it will be useful, |
| 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 14 | GNU General Public License for more details. |
| 15 | |
| 16 | You should have received a copy of the GNU General Public License |
| 17 | along with MoNav. If not, see <http://www.gnu.org/licenses/>. |
| 18 | */ |
| 19 | |
| 20 | #ifndef CONTRACTOR_H_INCLUDED |
| 21 | #define CONTRACTOR_H_INCLUDED |
| 22 | #include <vector> |
| 23 | #include <omp.h> |
| 24 | #include <limits> |
| 25 | #include "utils/qthelpers.h" |
| 26 | #include "dynamicgraph.h" |
| 27 | #include "binaryheap.h" |
| 28 | #include "utils/config.h" |
| 29 | |
| 30 | class Contractor { |
| 31 | |
| 32 | public: |
| 33 | |
| 34 | struct Witness { |
| 35 | NodeID source; |
| 36 | NodeID target; |
| 37 | NodeID middle; |
| 38 | }; |
| 39 | |
| 40 | private: |
| 41 | |
| 42 | struct _EdgeData { |
| 43 | unsigned distance; |
| 44 | unsigned originalEdges : 29; |
| 45 | bool shortcut : 1; |
| 46 | bool forward : 1; |
| 47 | bool backward : 1; |
| 48 | union { |
| 49 | NodeID middle; // shortcut |
| 50 | unsigned id; // original edge |
| 51 | }; |
| 52 | } data; |
| 53 | |
| 54 | struct _HeapData { |
| 55 | }; |
| 56 | |
| 57 | typedef DynamicGraph< _EdgeData > _DynamicGraph; |
| 58 | typedef BinaryHeap< NodeID, NodeID, unsigned, _HeapData > _Heap; |
| 59 | typedef _DynamicGraph::InputEdge _ImportEdge; |
| 60 | |
| 61 | struct _ThreadData { |
| 62 | _Heap heap; |
| 63 | std::vector< _ImportEdge > insertedEdges; |
| 64 | std::vector< Witness > witnessList; |
| 65 | std::vector< NodeID > neighbours; |
| 66 | _ThreadData( NodeID nodes ): heap( nodes ) { |
| 67 | } |
| 68 | }; |
| 69 | |
| 70 | struct _PriorityData { |
| 71 | int depth; |
| 72 | NodeID bias; |
| 73 | _PriorityData() { |
| 74 | depth = 0; |
| 75 | } |
| 76 | }; |
| 77 | |
| 78 | struct _ContractionInformation { |
| 79 | int edgesDeleted; |
| 80 | int edgesAdded; |
| 81 | int originalEdgesDeleted; |
| 82 | int originalEdgesAdded; |
| 83 | _ContractionInformation() { |
| 84 | edgesAdded = edgesDeleted = originalEdgesAdded = originalEdgesDeleted = 0; |
| 85 | } |
| 86 | }; |
| 87 | |
| 88 | struct _NodePartitionor { |
| 89 | bool operator()( std::pair< NodeID, bool > nodeData ) { |
| 90 | return !nodeData.second; |
| 91 | } |
| 92 | }; |
| 93 | |
| 94 | struct _LogItem { |
| 95 | unsigned iteration; |
| 96 | NodeID nodes; |
| 97 | double contraction; |
| 98 | double independent; |
| 99 | double inserting; |
| 100 | double removing; |
| 101 | double updating; |
| 102 | |
| 103 | _LogItem() { |
| 104 | iteration = nodes = contraction = independent = inserting = removing = updating = 0; |
| 105 | } |
| 106 | |
| 107 | double GetTotalTime() const { |
| 108 | return contraction + independent + inserting + removing + updating; |
| 109 | } |
| 110 | |
| 111 | void PrintStatistics() const { |
| 112 | qDebug( "%d\t%d\t%lf\t%lf\t%lf\t%lf\t%lf", iteration, nodes, independent, contraction, inserting, removing, updating ); |
| 113 | } |
| 114 | }; |
| 115 | |
| 116 | class _LogData { |
| 117 | public: |
| 118 | |
| 119 | std::vector < _LogItem > iterations; |
| 120 | |
| 121 | unsigned GetNIterations() { |
| 122 | return ( unsigned ) iterations.size(); |
| 123 | } |
| 124 | |
| 125 | _LogItem GetSum() const { |
| 126 | _LogItem sum; |
| 127 | sum.iteration = ( unsigned ) iterations.size(); |
| 128 | |
| 129 | for ( int i = 0, e = ( int ) iterations.size(); i < e; ++i ) { |
| 130 | sum.nodes += iterations[i].nodes; |
| 131 | sum.contraction += iterations[i].contraction; |
| 132 | sum.independent += iterations[i].independent; |
| 133 | sum.inserting += iterations[i].inserting; |
| 134 | sum.removing += iterations[i].removing; |
| 135 | sum.updating += iterations[i].updating; |
| 136 | } |
| 137 | |
| 138 | return sum; |
| 139 | } |
| 140 | |
| 141 | void PrintHeader() const { |
| 142 | qDebug( "Iteration\tNodes\tIndependent\tContraction\tInserting\tRemoving\tUpdating" ); |
| 143 | } |
| 144 | |
| 145 | void PrintSummary() const { |
| 146 | PrintHeader(); |
| 147 | GetSum().PrintStatistics(); |
| 148 | } |
| 149 | |
| 150 | void Print() const { |
| 151 | PrintHeader(); |
| 152 | for ( int i = 0, e = ( int ) iterations.size(); i < e; ++i ) |
| 153 | iterations[i].PrintStatistics(); |
| 154 | } |
| 155 | |
| 156 | void Insert( const _LogItem& data ) { |
| 157 | iterations.push_back( data ); |
| 158 | } |
| 159 | |
| 160 | }; |
| 161 | |
| 162 | public: |
| 163 | |
| 164 | template< class InputEdge > |
| 165 | Contractor( int nodes, const std::vector< InputEdge >& inputEdges ) { |
| 166 | std::vector< _ImportEdge > edges; |
| 167 | edges.reserve( 2 * inputEdges.size() ); |
| 168 | int skippedLargeEdges = 0; |
| 169 | for ( typename std::vector< InputEdge >::const_iterator i = inputEdges.begin(), e = inputEdges.end(); i != e; ++i ) { |
| 170 | _ImportEdge edge; |
| 171 | edge.source = i->source; |
| 172 | edge.target = i->target; |
| 173 | edge.data.distance = std::max( i->distance * 10.0 + 0.5, 1.0 ); |
| 174 | if ( edge.data.distance > 24 * 60 * 60 * 10 ) { |
| 175 | skippedLargeEdges++; |
| 176 | continue; |
| 177 | } |
| 178 | edge.data.shortcut = false; |
| 179 | edge.data.id = i - inputEdges.begin(); |
| 180 | edge.data.forward = true; |
| 181 | edge.data.backward = i->bidirectional; |
| 182 | edge.data.originalEdges = 1; |
| 183 | |
| 184 | if ( edge.data.distance < 1 ) { |
| 185 | qDebug() << edge.source << edge.target << edge.data.forward << edge.data.backward << edge.data.distance << edge.data.id << i->distance; |
| 186 | } |
| 187 | |
| 188 | if ( edge.source == edge.target ) { |
| 189 | _loops.push_back( edge ); |
| 190 | continue; |
| 191 | } |
| 192 | |
| 193 | edges.push_back( edge ); |
| 194 | std::swap( edge.source, edge.target ); |
| 195 | edge.data.forward = i->bidirectional; |
| 196 | edge.data.backward = true; |
| 197 | edges.push_back( edge ); |
| 198 | } |
| 199 | if ( skippedLargeEdges != 0 ) |
| 200 | qDebug( "Skipped %d edges with too large edge weight", skippedLargeEdges ); |
| 201 | std::sort( edges.begin(), edges.end() ); |
| 202 | |
| 203 | _graph = new _DynamicGraph( nodes, edges ); |
| 204 | |
| 205 | std::vector< _ImportEdge >().swap( edges ); |
| 206 | } |
| 207 | |
| 208 | ~Contractor() { |
| 209 | delete _graph; |
| 210 | } |
| 211 | |
| 212 | void Run() { |
| 213 | const NodeID numberOfNodes = _graph->GetNumberOfNodes(); |
| 214 | _LogData log; |
| 215 | |
| 216 | int maxThreads = omp_get_max_threads(); |
| 217 | std::vector < _ThreadData* > threadData; |
| 218 | for ( int threadNum = 0; threadNum < maxThreads; ++threadNum ) { |
| 219 | threadData.push_back( new _ThreadData( numberOfNodes ) ); |
| 220 | } |
| 221 | qDebug( "%d nodes, %d edges", numberOfNodes, _graph->GetNumberOfEdges() ); |
| 222 | qDebug( "using %d threads", maxThreads ); |
| 223 | |
| 224 | NodeID levelID = 0; |
| 225 | NodeID iteration = 0; |
| 226 | std::vector< std::pair< NodeID, bool > > remainingNodes( numberOfNodes ); |
| 227 | std::vector< double > nodePriority( numberOfNodes ); |
| 228 | std::vector< _PriorityData > nodeData( numberOfNodes ); |
| 229 | |
| 230 | //initialize the variables |
| 231 | #pragma omp parallel for schedule ( guided ) |
| 232 | for ( int x = 0; x < ( int ) numberOfNodes; ++x ) |
| 233 | remainingNodes[x].first = x; |
| 234 | std::random_shuffle( remainingNodes.begin(), remainingNodes.end() ); |
| 235 | for ( int x = 0; x < ( int ) numberOfNodes; ++x ) |
| 236 | nodeData[remainingNodes[x].first].bias = x; |
| 237 | |
| 238 | qDebug( "Initialise Elimination PQ... " ); |
| 239 | _LogItem statistics0; |
| 240 | statistics0.updating = _Timestamp(); |
| 241 | statistics0.iteration = 0; |
| 242 | #pragma omp parallel |
| 243 | { |
| 244 | _ThreadData* data = threadData[omp_get_thread_num()]; |
| 245 | #pragma omp for schedule ( guided ) |
| 246 | for ( int x = 0; x < ( int ) numberOfNodes; ++x ) { |
| 247 | nodePriority[x] = _Evaluate( data, &nodeData[x], x ); |
| 248 | } |
| 249 | } |
| 250 | qDebug( "done" ); |
| 251 | |
| 252 | statistics0.updating = _Timestamp() - statistics0.updating; |
| 253 | log.Insert( statistics0 ); |
| 254 | |
| 255 | log.PrintHeader(); |
| 256 | statistics0.PrintStatistics(); |
| 257 | |
| 258 | while ( levelID < numberOfNodes ) { |
| 259 | _LogItem statistics; |
| 260 | statistics.iteration = iteration++; |
| 261 | const int last = ( int ) remainingNodes.size(); |
| 262 | |
| 263 | //determine independent node set |
| 264 | double timeLast = _Timestamp(); |
| 265 | #pragma omp parallel |
| 266 | { |
| 267 | _ThreadData* const data = threadData[omp_get_thread_num()]; |
| 268 | #pragma omp for schedule ( guided ) |
| 269 | for ( int i = 0; i < last; ++i ) { |
| 270 | const NodeID node = remainingNodes[i].first; |
| 271 | remainingNodes[i].second = _IsIndependent( nodePriority, nodeData, data, node ); |
| 272 | } |
| 273 | } |
| 274 | _NodePartitionor functor; |
| 275 | const std::vector < std::pair < NodeID, bool > >::const_iterator first = stable_partition( remainingNodes.begin(), remainingNodes.end(), functor ); |
| 276 | const int firstIndependent = first - remainingNodes.begin(); |
| 277 | statistics.nodes = last - firstIndependent; |
| 278 | statistics.independent += _Timestamp() - timeLast; |
| 279 | timeLast = _Timestamp(); |
| 280 | |
| 281 | //contract independent nodes |
| 282 | #pragma omp parallel |
| 283 | { |
| 284 | _ThreadData* const data = threadData[omp_get_thread_num()]; |
| 285 | #pragma omp for schedule ( guided ) nowait |
| 286 | for ( int position = firstIndependent ; position < last; ++position ) { |
| 287 | NodeID x = remainingNodes[position].first; |
| 288 | _Contract< false > ( data, x ); |
| 289 | nodePriority[x] = -1; |
| 290 | } |
| 291 | std::sort( data->insertedEdges.begin(), data->insertedEdges.end() ); |
| 292 | } |
| 293 | statistics.contraction += _Timestamp() - timeLast; |
| 294 | timeLast = _Timestamp(); |
| 295 | |
| 296 | #pragma omp parallel |
| 297 | { |
| 298 | _ThreadData* const data = threadData[omp_get_thread_num()]; |
| 299 | #pragma omp for schedule ( guided ) nowait |
| 300 | for ( int position = firstIndependent ; position < last; ++position ) { |
| 301 | NodeID x = remainingNodes[position].first; |
| 302 | _DeleteIncommingEdges( data, x ); |
| 303 | } |
| 304 | } |
| 305 | statistics.removing += _Timestamp() - timeLast; |
| 306 | timeLast = _Timestamp(); |
| 307 | |
| 308 | //insert new edges |
| 309 | for ( int threadNum = 0; threadNum < maxThreads; ++threadNum ) { |
| 310 | _ThreadData& data = *threadData[threadNum]; |
| 311 | for ( int i = 0; i < ( int ) data.insertedEdges.size(); ++i ) { |
| 312 | const _ImportEdge& edge = data.insertedEdges[i]; |
| 313 | _graph->InsertEdge( edge.source, edge.target, edge.data ); |
| 314 | } |
| 315 | std::vector< _ImportEdge >().swap( data.insertedEdges ); |
| 316 | } |
| 317 | statistics.inserting += _Timestamp() - timeLast; |
| 318 | timeLast = _Timestamp(); |
| 319 | |
| 320 | //update priorities |
| 321 | #pragma omp parallel |
| 322 | { |
| 323 | _ThreadData* const data = threadData[omp_get_thread_num()]; |
| 324 | #pragma omp for schedule ( guided ) nowait |
| 325 | for ( int position = firstIndependent ; position < last; ++position ) { |
| 326 | NodeID x = remainingNodes[position].first; |
| 327 | _UpdateNeighbours( &nodePriority, &nodeData, data, x ); |
| 328 | } |
| 329 | } |
| 330 | statistics.updating += _Timestamp() - timeLast; |
| 331 | timeLast = _Timestamp(); |
| 332 | |
| 333 | //output some statistics |
| 334 | statistics.PrintStatistics(); |
| 335 | //qDebug( wxT( "Printed" ) ); |
| 336 | |
| 337 | //remove contracted nodes from the pool |
| 338 | levelID += last - firstIndependent; |
| 339 | remainingNodes.resize( firstIndependent ); |
| 340 | std::vector< std::pair< NodeID, bool > >( remainingNodes ).swap( remainingNodes ); |
| 341 | log.Insert( statistics ); |
| 342 | } |
| 343 | |
| 344 | for ( int threadNum = 0; threadNum < maxThreads; threadNum++ ) { |
| 345 | _witnessList.insert( _witnessList.end(), threadData[threadNum]->witnessList.begin(), threadData[threadNum]->witnessList.end() ); |
| 346 | delete threadData[threadNum]; |
| 347 | } |
| 348 | |
| 349 | log.PrintSummary(); |
| 350 | qDebug( "Total Time: %lf s", log.GetSum().GetTotalTime() ); |
| 351 | |
| 352 | } |
| 353 | |
| 354 | template< class Edge > |
| 355 | void GetEdges( std::vector< Edge >* edges ) { |
| 356 | NodeID numberOfNodes = _graph->GetNumberOfNodes(); |
| 357 | for ( NodeID node = 0; node < numberOfNodes; ++node ) { |
| 358 | for ( _DynamicGraph::EdgeIterator edge = _graph->BeginEdges( node ), endEdges = _graph->EndEdges( node ); edge != endEdges; ++edge ) { |
| 359 | const NodeID target = _graph->GetTarget( edge ); |
| 360 | const _EdgeData& data = _graph->GetEdgeData( edge ); |
| 361 | Edge newEdge; |
| 362 | newEdge.source = node; |
| 363 | newEdge.target = target; |
| 364 | newEdge.data.distance = data.distance; |
| 365 | newEdge.data.shortcut = data.shortcut; |
| 366 | if ( data.shortcut ) |
| 367 | newEdge.data.middle = data.middle; |
| 368 | else |
| 369 | newEdge.data.id = data.id; |
| 370 | newEdge.data.forward = data.forward; |
| 371 | newEdge.data.backward = data.backward; |
| 372 | edges->push_back( newEdge ); |
| 373 | } |
| 374 | } |
| 375 | } |
| 376 | |
| 377 | template< class Edge > |
| 378 | void GetLoops( std::vector< Edge >* edges ) { |
| 379 | for ( unsigned i = 0; i < _loops.size(); i++ ) { |
| 380 | Edge newEdge; |
| 381 | newEdge.source = _loops[i].source; |
| 382 | newEdge.target = _loops[i].target; |
| 383 | newEdge.data.distance = _loops[i].data.distance; |
| 384 | newEdge.data.shortcut = _loops[i].data.shortcut; |
| 385 | newEdge.data.id = _loops[i].data.id; |
| 386 | newEdge.data.forward = _loops[i].data.forward; |
| 387 | newEdge.data.backward = _loops[i].data.backward; |
| 388 | edges->push_back( newEdge ); |
| 389 | } |
| 390 | } |
| 391 | |
| 392 | void GetWitnessList( std::vector< Witness >& list ) { |
| 393 | list = _witnessList; |
| 394 | } |
| 395 | |
| 396 | private: |
| 397 | |
| 398 | double _Timestamp() { |
| 399 | static Timer timer; |
| 400 | return ( double ) timer.elapsed() / 1000; |
| 401 | } |
| 402 | |
| 403 | bool _ConstructCH( _DynamicGraph* _graph ); |
| 404 | |
| 405 | void _Dijkstra( const unsigned maxDistance, const int maxNodes, _ThreadData* const data ){ |
| 406 | |
| 407 | _Heap& heap = data->heap; |
| 408 | |
| 409 | int nodes = 0; |
| 410 | while ( heap.Size() > 0 ) { |
| 411 | const NodeID node = heap.DeleteMin(); |
| 412 | const unsigned distance = heap.GetKey( node ); |
| 413 | if ( nodes++ > maxNodes ) |
| 414 | return; |
| 415 | //Destination settled? |
| 416 | if ( distance > maxDistance ) |
| 417 | return; |
| 418 | |
| 419 | //iterate over all edges of node |
| 420 | for ( _DynamicGraph::EdgeIterator edge = _graph->BeginEdges( node ), endEdges = _graph->EndEdges( node ); edge != endEdges; ++edge ) { |
| 421 | const _EdgeData& data = _graph->GetEdgeData( edge ); |
| 422 | if ( !data.forward ) |
| 423 | continue; |
| 424 | const NodeID to = _graph->GetTarget( edge ); |
| 425 | const unsigned toDistance = distance + data.distance; |
| 426 | |
| 427 | //New Node discovered -> Add to Heap + Node Info Storage |
| 428 | if ( !heap.WasInserted( to ) ) |
| 429 | heap.Insert( to, toDistance, _HeapData() ); |
| 430 | |
| 431 | //Found a shorter Path -> Update distance |
| 432 | else if ( toDistance < heap.GetKey( to ) ) { |
| 433 | heap.DecreaseKey( to, toDistance ); |
| 434 | } |
| 435 | } |
| 436 | } |
| 437 | } |
| 438 | |
| 439 | double _Evaluate( _ThreadData* const data, _PriorityData* const nodeData, NodeID node ){ |
| 440 | _ContractionInformation stats; |
| 441 | |
| 442 | //perform simulated contraction |
| 443 | _Contract< true > ( data, node, &stats ); |
| 444 | |
| 445 | // Result will contain the priority |
| 446 | double result; |
| 447 | if ( stats.edgesDeleted == 0 || stats.originalEdgesDeleted == 0 ) |
| 448 | result = 1 * nodeData->depth; |
| 449 | else |
| 450 | result = 2 * ((( double ) stats.edgesAdded ) / stats.edgesDeleted ) + 1 * ((( double ) stats.originalEdgesAdded ) / stats.originalEdgesDeleted ) + 1 * nodeData->depth; |
| 451 | assert( result >= 0 ); |
| 452 | return result; |
| 453 | } |
| 454 | |
| 455 | |
| 456 | template< bool Simulate > bool _Contract( _ThreadData* const data, NodeID node, _ContractionInformation* const stats = NULL ) { |
| 457 | _Heap& heap = data->heap; |
| 458 | //std::vector< Witness >& witnessList = data->witnessList; |
| 459 | int insertedEdgesSize = data->insertedEdges.size(); |
| 460 | std::vector< _ImportEdge >& insertedEdges = data->insertedEdges; |
| 461 | |
| 462 | for ( _DynamicGraph::EdgeIterator inEdge = _graph->BeginEdges( node ), endInEdges = _graph->EndEdges( node ); inEdge != endInEdges; ++inEdge ) { |
| 463 | const _EdgeData& inData = _graph->GetEdgeData( inEdge ); |
| 464 | const NodeID source = _graph->GetTarget( inEdge ); |
| 465 | if ( Simulate ) { |
| 466 | assert( stats != NULL ); |
| 467 | stats->edgesDeleted++; |
| 468 | stats->originalEdgesDeleted += inData.originalEdges; |
| 469 | } |
| 470 | if ( !inData.backward ) |
| 471 | continue; |
| 472 | |
| 473 | heap.Clear(); |
| 474 | heap.Insert( source, 0, _HeapData() ); |
| 475 | if ( node != source ) |
| 476 | heap.Insert( node, inData.distance, _HeapData() ); |
| 477 | unsigned maxDistance = 0; |
| 478 | |
| 479 | for ( _DynamicGraph::EdgeIterator outEdge = _graph->BeginEdges( node ), endOutEdges = _graph->EndEdges( node ); outEdge != endOutEdges; ++outEdge ) { |
| 480 | const _EdgeData& outData = _graph->GetEdgeData( outEdge ); |
| 481 | if ( !outData.forward ) |
| 482 | continue; |
| 483 | const NodeID target = _graph->GetTarget( outEdge ); |
| 484 | const unsigned pathDistance = inData.distance + outData.distance; |
| 485 | maxDistance = std::max( maxDistance, pathDistance ); |
| 486 | if ( !heap.WasInserted( target ) ) |
| 487 | heap.Insert( target, pathDistance, _HeapData() ); |
| 488 | else if ( pathDistance < heap.GetKey( target ) ) |
| 489 | heap.DecreaseKey( target, pathDistance ); |
| 490 | } |
| 491 | |
| 492 | if ( Simulate ) |
| 493 | _Dijkstra( maxDistance, 500, data ); |
| 494 | else |
| 495 | _Dijkstra( maxDistance, 1000, data ); |
| 496 | |
| 497 | for ( _DynamicGraph::EdgeIterator outEdge = _graph->BeginEdges( node ), endOutEdges = _graph->EndEdges( node ); outEdge != endOutEdges; ++outEdge ) { |
| 498 | const _EdgeData& outData = _graph->GetEdgeData( outEdge ); |
| 499 | if ( !outData.forward ) |
| 500 | continue; |
| 501 | const NodeID target = _graph->GetTarget( outEdge ); |
| 502 | const int pathDistance = inData.distance + outData.distance; |
| 503 | const int distance = heap.GetKey( target ); |
| 504 | |
| 505 | if ( pathDistance <= distance ) { |
| 506 | if ( Simulate ) { |
| 507 | assert( stats != NULL ); |
| 508 | stats->edgesAdded += 2; |
| 509 | stats->originalEdgesAdded += 2 * ( outData.originalEdges + inData.originalEdges ); |
| 510 | } else { |
| 511 | _ImportEdge newEdge; |
| 512 | newEdge.source = source; |
| 513 | newEdge.target = target; |
| 514 | newEdge.data.distance = pathDistance; |
| 515 | newEdge.data.forward = true; |
| 516 | newEdge.data.backward = false; |
| 517 | newEdge.data.middle = node; |
| 518 | newEdge.data.shortcut = true; |
| 519 | newEdge.data.originalEdges = outData.originalEdges + inData.originalEdges; |
| 520 | insertedEdges.push_back( newEdge ); |
| 521 | std::swap( newEdge.source, newEdge.target ); |
| 522 | newEdge.data.forward = false; |
| 523 | newEdge.data.backward = true; |
| 524 | insertedEdges.push_back( newEdge ); |
| 525 | } |
| 526 | } |
| 527 | /*else if ( !Simulate ) { |
| 528 | Witness witness; |
| 529 | witness.source = source; |
| 530 | witness.target = target; |
| 531 | witness.middle = node; |
| 532 | witnessList.push_back( witness ); |
| 533 | }*/ |
| 534 | } |
| 535 | } |
| 536 | |
| 537 | if ( !Simulate ) { |
| 538 | for ( int i = insertedEdgesSize, iend = insertedEdges.size(); i < iend; i++ ) { |
| 539 | bool found = false; |
| 540 | for ( int other = i + 1 ; other < iend ; ++other ) { |
| 541 | if ( insertedEdges[other].source != insertedEdges[i].source ) |
| 542 | continue; |
| 543 | if ( insertedEdges[other].target != insertedEdges[i].target ) |
| 544 | continue; |
| 545 | if ( insertedEdges[other].data.distance != insertedEdges[i].data.distance ) |
| 546 | continue; |
| 547 | if ( insertedEdges[other].data.shortcut != insertedEdges[i].data.shortcut ) |
| 548 | continue; |
| 549 | insertedEdges[other].data.forward |= insertedEdges[i].data.forward; |
| 550 | insertedEdges[other].data.backward |= insertedEdges[i].data.backward; |
| 551 | found = true; |
| 552 | break; |
| 553 | } |
| 554 | if ( !found ) |
| 555 | insertedEdges[insertedEdgesSize++] = insertedEdges[i]; |
| 556 | } |
| 557 | insertedEdges.resize( insertedEdgesSize ); |
| 558 | } |
| 559 | |
| 560 | return true; |
| 561 | } |
| 562 | |
| 563 | bool _DeleteIncommingEdges( _ThreadData* const data, NodeID node ) { |
| 564 | std::vector< NodeID >& neighbours = data->neighbours; |
| 565 | neighbours.clear(); |
| 566 | |
| 567 | //find all neighbours |
| 568 | for ( _DynamicGraph::EdgeIterator e = _graph->BeginEdges( node ) ; e < _graph->EndEdges( node ) ; ++e ) { |
| 569 | const NodeID u = _graph->GetTarget( e ); |
| 570 | if ( u == node ) |
| 571 | continue; |
| 572 | neighbours.push_back( u ); |
| 573 | } |
| 574 | //eliminate duplicate entries ( forward + backward edges ) |
| 575 | std::sort( neighbours.begin(), neighbours.end() ); |
| 576 | neighbours.resize( std::unique( neighbours.begin(), neighbours.end() ) - neighbours.begin() ); |
| 577 | |
| 578 | for ( int i = 0, e = ( int ) neighbours.size(); i < e; ++i ) { |
| 579 | const NodeID u = neighbours[i]; |
| 580 | _graph->DeleteEdgesTo( u, node ); |
| 581 | } |
| 582 | |
| 583 | return true; |
| 584 | } |
| 585 | |
| 586 | bool _UpdateNeighbours( std::vector< double >* priorities, std::vector< _PriorityData >* const nodeData, _ThreadData* const data, NodeID node ) { |
| 587 | std::vector< NodeID >& neighbours = data->neighbours; |
| 588 | neighbours.clear(); |
| 589 | |
| 590 | //find all neighbours |
| 591 | for ( _DynamicGraph::EdgeIterator e = _graph->BeginEdges( node ) ; e < _graph->EndEdges( node ) ; ++e ) { |
| 592 | const NodeID u = _graph->GetTarget( e ); |
| 593 | if ( u == node ) |
| 594 | continue; |
| 595 | neighbours.push_back( u ); |
| 596 | ( *nodeData )[u].depth = std::max(( *nodeData )[node].depth + 1, ( *nodeData )[u].depth ); |
| 597 | } |
| 598 | //eliminate duplicate entries ( forward + backward edges ) |
| 599 | std::sort( neighbours.begin(), neighbours.end() ); |
| 600 | neighbours.resize( std::unique( neighbours.begin(), neighbours.end() ) - neighbours.begin() ); |
| 601 | |
| 602 | for ( int i = 0, e = ( int ) neighbours.size(); i < e; ++i ) { |
| 603 | const NodeID u = neighbours[i]; |
| 604 | ( *priorities )[u] = _Evaluate( data, &( *nodeData )[u], u ); |
| 605 | } |
| 606 | |
| 607 | return true; |
| 608 | } |
| 609 | |
| 610 | bool _IsIndependent( const std::vector< double >& priorities, const std::vector< _PriorityData >& nodeData, _ThreadData* const data, NodeID node ) { |
| 611 | const double priority = priorities[node]; |
| 612 | |
| 613 | std::vector< NodeID >& neighbours = data->neighbours; |
| 614 | neighbours.clear(); |
| 615 | |
| 616 | for ( _DynamicGraph::EdgeIterator e = _graph->BeginEdges( node ) ; e < _graph->EndEdges( node ) ; ++e ) { |
| 617 | const NodeID target = _graph->GetTarget( e ); |
| 618 | const double targetPriority = priorities[target]; |
| 619 | assert( targetPriority >= 0 ); |
| 620 | //found a neighbour with lower priority? |
| 621 | if ( priority > targetPriority ) |
| 622 | return false; |
| 623 | //tie breaking |
| 624 | if ( priority == targetPriority && nodeData[node].bias < nodeData[target].bias ) |
| 625 | return false; |
| 626 | neighbours.push_back( target ); |
| 627 | } |
| 628 | |
| 629 | std::sort( neighbours.begin(), neighbours.end() ); |
| 630 | neighbours.resize( std::unique( neighbours.begin(), neighbours.end() ) - neighbours.begin() ); |
| 631 | |
| 632 | //examine all neighbours that are at most 2 hops away |
| 633 | for ( std::vector< NodeID >::const_iterator i = neighbours.begin(), lastNode = neighbours.end(); i != lastNode; ++i ) { |
| 634 | const NodeID u = *i; |
| 635 | |
| 636 | for ( _DynamicGraph::EdgeIterator e = _graph->BeginEdges( u ) ; e < _graph->EndEdges( u ) ; ++e ) { |
| 637 | const NodeID target = _graph->GetTarget( e ); |
| 638 | |
| 639 | const double targetPriority = priorities[target]; |
| 640 | assert( targetPriority >= 0 ); |
| 641 | //found a neighbour with lower priority? |
| 642 | if ( priority > targetPriority ) |
| 643 | return false; |
| 644 | //tie breaking |
| 645 | if ( priority == targetPriority && nodeData[node].bias < nodeData[target].bias ) |
| 646 | return false; |
| 647 | } |
| 648 | } |
| 649 | |
| 650 | return true; |
| 651 | } |
| 652 | |
| 653 | |
| 654 | _DynamicGraph* _graph; |
| 655 | std::vector< Witness > _witnessList; |
| 656 | std::vector< _ImportEdge > _loops; |
| 657 | }; |
| 658 | |
| 659 | #endif // CONTRACTOR_H_INCLUDED |
| 660 |
Branches:
master
