- /* TODO
- std::vector< long > fpath;
- fpath.push_back( ia );
- while( ia != ib )
- {
- ia = this->m_FrontPaths[ ia ][ ib ];
- fpath.push_back( ia );
-
- } // elihw
-
- // Continue only if both fronts are connected
- unsigned int N = fpath.size( );
- if( N > 0 )
- {
- // First path: from start vertex to first collision
- this->GetPath(
- path, a,
- this->m_Collisions[ fpath[ 0 ] ][ fpath[ 1 ] ].first
- );
-
- // Intermediary paths
- for( unsigned int i = 1; i < N - 1; ++i )
- {
- TPathTVertices ipath =
- this->GetPath(
- this->m_Collisions[ fpath[ i ] ][ fpath[ i - 1 ] ].first,
- this->m_Collisions[ fpath[ i ] ][ fpath[ i + 1 ] ].first
- );
- path.insert( path.end( ), ipath.begin( ), ipath.end( ) );
-
- } // rof
-
- // Final path: from last collision to end point
- TVertices lpath =
- this->GetPath(
- this->m_Collisions[ fpath[ N - 1 ] ][ fpath[ N - 2 ] ].first, b
- );
- path.insert( path.end( ), lpath.begin( ), lpath.end( ) );
-
- } // fi
-
- } // fi
- */
+ std::vector< long > fpath;
+ fpath.push_back( ia );
+ while( ia != ib )
+ {
+ ia = this->m_FrontPaths[ ia ][ ib ];
+ fpath.push_back( ia );
+
+ } // elihw
+
+ // Continue only if both fronts are connected
+ unsigned int N = fpath.size( );
+ if( N > 0 )
+ {
+ // First path: from start vertex to first collision
+ vertices = this->GetPath(
+ a, this->m_Collisions[ fpath[ 0 ] ][ fpath[ 1 ] ].first
+ );
+
+ // Intermediary paths
+ for( unsigned int i = 1; i < N - 1; ++i )
+ {
+ TVertices ipath =
+ this->GetPath(
+ this->m_Collisions[ fpath[ i ] ][ fpath[ i - 1 ] ].first,
+ this->m_Collisions[ fpath[ i ] ][ fpath[ i + 1 ] ].first
+ );
+ for( long id = 0; id < ipath.size( ); ++id )
+ vertices.push_back( ipath[ id ] );
+
+ } // rof
+
+ // Final path: from last collision to end point
+ TVertices lpath =
+ this->GetPath(
+ this->m_Collisions[ fpath[ N - 1 ] ][ fpath[ N - 2 ] ].first, b
+ );
+ for( long id = 0; id < lpath.size( ); ++id )
+ vertices.push_back( lpath[ id ] );
+
+ } // fi