1 #ifndef __fpa__Base__MinimumSpanningTree__hxx__
2 #define __fpa__Base__MinimumSpanningTree__hxx__
6 // -------------------------------------------------------------------------
7 template< class _TVertex, class _TSuperclass >
8 const typename fpa::Base::MinimumSpanningTree< _TVertex, _TSuperclass >::
9 TCollisions& fpa::Base::MinimumSpanningTree< _TVertex, _TSuperclass >::
10 GetCollisions( ) const
12 return( this->m_Collisions );
15 // -------------------------------------------------------------------------
16 template< class _TVertex, class _TSuperclass >
17 void fpa::Base::MinimumSpanningTree< _TVertex, _TSuperclass >::
18 SetCollisions( const TCollisions& collisions )
20 static const unsigned long _inf =
21 std::numeric_limits< unsigned long >::max( );
22 if( this->m_Collisions == collisions )
25 this->m_Collisions = collisions;
27 // Prepare a front graph
28 unsigned long N = this->m_Collisions.size( );
29 _TMatrix dist( N, _TRow( N, _inf ) );
30 this->m_FrontPaths = dist;
31 for( unsigned long i = 0; i < N; ++i )
33 for( unsigned long j = 0; j < N; ++j )
35 if( this->m_Collisions[ i ][ j ].second )
39 this->m_FrontPaths[ i ][ j ] = j;
40 this->m_FrontPaths[ j ][ i ] = i;
46 this->m_FrontPaths[ i ][ i ] = i;
50 // Use Floyd-Warshall to compute all possible paths between fronts
51 for( unsigned long k = 0; k < N; ++k )
53 for( unsigned long i = 0; i < N; ++i )
55 for( unsigned long j = 0; j < N; ++j )
57 // WARNING: you don't want a numeric overflow!!!
58 unsigned long dik = dist[ i ][ k ];
59 unsigned long dkj = dist[ k ][ j ];
60 unsigned long sum = _inf;
61 if( dik < _inf && dkj < _inf )
64 // Ok, continue Floyd-Warshall
65 if( sum < dist[ i ][ j ] )
68 this->m_FrontPaths[ i ][ j ] = this->m_FrontPaths[ i ][ k ];
80 // -------------------------------------------------------------------------
81 template< class _TVertex, class _TSuperclass >
82 void fpa::Base::MinimumSpanningTree< _TVertex, _TSuperclass >::
85 this->m_Seeds.clear( );
89 // -------------------------------------------------------------------------
90 template< class _TVertex, class _TSuperclass >
91 void fpa::Base::MinimumSpanningTree< _TVertex, _TSuperclass >::
92 AddSeed( const _TVertex& seed )
94 this->m_Seeds.push_back( seed );
98 // -------------------------------------------------------------------------
99 template< class _TVertex, class _TSuperclass >
100 typename fpa::Base::MinimumSpanningTree< _TVertex, _TSuperclass >::
101 TVertices fpa::Base::MinimumSpanningTree< _TVertex, _TSuperclass >::
102 GetPath( const _TVertex& a ) const
106 _TVertex p = this->GetParent( it );
109 path.push_front( it );
111 p = this->GetParent( it );
114 path.push_front( it );
118 // -------------------------------------------------------------------------
119 template< class _TVertex, class _TSuperclass >
120 typename fpa::Base::MinimumSpanningTree< _TVertex, _TSuperclass >::
121 TVertices fpa::Base::MinimumSpanningTree< _TVertex, _TSuperclass >::
122 GetPath( const _TVertex& a, const _TVertex& b ) const
124 static const unsigned long _inf =
125 std::numeric_limits< unsigned long >::max( );
128 TVertices pa = this->GetPath( a );
129 TVertices pb = this->GetPath( b );
130 if( pa.size( ) > 0 && pb.size( ) > 0 )
132 // Find front identifiers
133 unsigned long ia = _inf, ib = _inf;
134 unsigned long N = this->m_Seeds.size( );
135 for( unsigned long i = 0; i < N; ++i )
137 if( this->m_Seeds[ i ] == pa.front( ) )
139 if( this->m_Seeds[ i ] == pb.front( ) )
146 // Use this->m_FrontPaths from Floyd-Warshall
147 if( this->m_FrontPaths[ ia ][ ib ] < _inf )
149 // Compute front path
150 std::vector< long > fpath;
151 fpath.push_back( ia );
154 ia = this->m_FrontPaths[ ia ][ ib ];
155 fpath.push_back( ia );
159 // Continue only if both fronts are connected
160 unsigned int N = fpath.size( );
163 // First path: from start vertex to first collision
164 path = this->GetPath(
165 a, this->m_Collisions[ fpath[ 0 ] ][ fpath[ 1 ] ].first
168 // Intermediary paths
169 for( unsigned int i = 1; i < N - 1; ++i )
173 this->m_Collisions[ fpath[ i ] ][ fpath[ i - 1 ] ].first,
174 this->m_Collisions[ fpath[ i ] ][ fpath[ i + 1 ] ].first
176 path.insert( path.end( ), ipath.begin( ), ipath.end( ) );
180 // Final path: from last collision to end point
183 this->m_Collisions[ fpath[ N - 1 ] ][ fpath[ N - 2 ] ].first, b
185 path.insert( path.end( ), lpath.begin( ), lpath.end( ) );
193 // Ignore common part: find common ancestor
194 auto aIt = pa.begin( );
195 auto bIt = pb.begin( );
196 while( *aIt == *bIt && aIt != pa.end( ) && bIt != pb.end( ) )
204 for( --aIt; aIt != pa.end( ); ++aIt )
205 path.push_front( *aIt );
206 for( ; bIt != pb.end( ); ++bIt )
207 path.push_back( *bIt );
215 // -------------------------------------------------------------------------
216 template< class _TVertex, class _TSuperclass >
217 fpa::Base::MinimumSpanningTree< _TVertex, _TSuperclass >::
218 MinimumSpanningTree( )
223 // -------------------------------------------------------------------------
224 template< class _TVertex, class _TSuperclass >
225 fpa::Base::MinimumSpanningTree< _TVertex, _TSuperclass >::
226 ~MinimumSpanningTree( )
230 #endif // __fpa__Base__MinimumSpanningTree__hxx__