1 #ifndef __fpa__Base__MinimumSpanningTree__hxx__
2 #define __fpa__Base__MinimumSpanningTree__hxx__
6 // -------------------------------------------------------------------------
7 template< class _TVertex, class _TPath, class _TSuperclass >
9 fpa::Base::MinimumSpanningTree< _TVertex, _TPath, _TSuperclass >::
11 fpa::Base::MinimumSpanningTree< _TVertex, _TPath, _TSuperclass >::
12 GetCollisions( ) const
14 return( this->m_Collisions );
17 // -------------------------------------------------------------------------
18 template< class _TVertex, class _TPath, class _TSuperclass >
19 void fpa::Base::MinimumSpanningTree< _TVertex, _TPath, _TSuperclass >::
20 SetCollisions( const TCollisions& collisions )
22 static const unsigned long _inf =
23 std::numeric_limits< unsigned long >::max( );
24 if( this->m_Collisions == collisions )
27 this->m_Collisions = collisions;
29 // Prepare a front graph
30 unsigned long N = this->m_Collisions.size( );
31 _TMatrix dist( N, _TRow( N, _inf ) );
32 this->m_FrontPaths = dist;
33 for( unsigned long i = 0; i < N; ++i )
35 for( unsigned long j = 0; j < N; ++j )
37 if( this->m_Collisions[ i ][ j ].second )
41 this->m_FrontPaths[ i ][ j ] = j;
42 this->m_FrontPaths[ j ][ i ] = i;
48 this->m_FrontPaths[ i ][ i ] = i;
52 // Use Floyd-Warshall to compute all possible paths between fronts
53 for( unsigned long k = 0; k < N; ++k )
55 for( unsigned long i = 0; i < N; ++i )
57 for( unsigned long j = 0; j < N; ++j )
59 // WARNING: you don't want a numeric overflow!!!
60 unsigned long dik = dist[ i ][ k ];
61 unsigned long dkj = dist[ k ][ j ];
62 unsigned long sum = _inf;
63 if( dik < _inf && dkj < _inf )
66 // Ok, continue Floyd-Warshall
67 if( sum < dist[ i ][ j ] )
70 this->m_FrontPaths[ i ][ j ] = this->m_FrontPaths[ i ][ k ];
82 // -------------------------------------------------------------------------
83 template< class _TVertex, class _TPath, class _TSuperclass >
84 void fpa::Base::MinimumSpanningTree< _TVertex, _TPath, _TSuperclass >::
87 this->m_Seeds.clear( );
91 // -------------------------------------------------------------------------
92 template< class _TVertex, class _TPath, class _TSuperclass >
93 void fpa::Base::MinimumSpanningTree< _TVertex, _TPath, _TSuperclass >::
94 AddSeed( const _TVertex& seed )
96 this->m_Seeds.push_back( seed );
100 // -------------------------------------------------------------------------
101 template< class _TVertex, class _TPath, class _TSuperclass >
102 void fpa::Base::MinimumSpanningTree< _TVertex, _TPath, _TSuperclass >::
103 GetPath( typename _TPath::Pointer& path, const _TVertex& a ) const
105 std::vector< _TVertex > vertices;
107 _TVertex p = this->GetParent( it );
110 vertices.push_back( it );
112 p = this->GetParent( it );
115 vertices.push_back( it );
118 path = _TPath::New( );
119 for( auto v = vertices.begin( ); v != vertices.end( ); ++v )
120 path->AddVertex( *v );
123 // -------------------------------------------------------------------------
124 template< class _TVertex, class _TPath, class _TSuperclass >
125 void fpa::Base::MinimumSpanningTree< _TVertex, _TPath, _TSuperclass >::
127 typename _TPath::Pointer& path, const _TVertex& a, const _TVertex& b
130 static const unsigned long _inf =
131 std::numeric_limits< unsigned long >::max( );
134 path = _TPath::New( );
135 typename _TPath::Pointer pa, pb;
136 this->GetPath( pa, a );
137 this->GetPath( pb, b );
138 if( pa->GetSize( ) > 0 && pb->GetSize( ) > 0 )
140 // Find front identifiers
141 unsigned long ia = _inf, ib = _inf;
142 unsigned long N = this->m_Seeds.size( );
143 for( unsigned long i = 0; i < N; ++i )
145 if( this->m_Seeds[ i ] == pa->GetVertex( pa->GetSize( ) - 1 ) )
147 if( this->m_Seeds[ i ] == pb->GetVertex( pb->GetSize( ) - 1 ) )
152 // Check if there is a front-jump between given seeds
155 // Compute front path
156 std::vector< long > fpath;
157 fpath.push_back( ia );
160 ia = this->m_FrontPaths[ ia ][ ib ];
161 fpath.push_back( ia );
165 // Continue only if both fronts are connected
166 unsigned int N = fpath.size( );
169 // First path: from start vertex to first collision
172 this->m_Collisions[ fpath[ 0 ] ][ fpath[ 1 ] ].first
175 // Intermediary paths
176 for( unsigned int i = 1; i < N - 1; ++i )
178 typename _TPath::Pointer ipath;
181 this->m_Collisions[ fpath[ i ] ][ fpath[ i - 1 ] ].first,
182 this->m_Collisions[ fpath[ i ] ][ fpath[ i + 1 ] ].first
184 for( long id = 0; id < ipath->GetSize( ); ++id )
185 path->AddVertex( ipath->GetVertex( id ) );
189 // Final path: from last collision to end point
190 typename _TPath::Pointer lpath;
193 this->m_Collisions[ fpath[ N - 1 ] ][ fpath[ N - 2 ] ].first, b
195 for( long id = 0; id < lpath->GetSize( ); ++id )
196 path->AddVertex( lpath->GetVertex( id ) );
202 // Ignore common part: find common ancestor
203 long aIt = pa->GetSize( ) - 1;
204 long bIt = pb->GetSize( ) - 1;
206 while( aIt >= 0 && bIt >= 0 && cont )
208 cont = ( pa->GetVertex( aIt ) == pb->GetVertex( bIt ) );
217 for( long cIt = 0; cIt <= aIt; ++cIt )
218 path->AddVertex( pa->GetVertex( cIt ) );
219 for( ; bIt >= 0; --bIt )
220 path->AddVertex( pb->GetVertex( bIt ) );
227 // -------------------------------------------------------------------------
228 template< class _TVertex, class _TPath, class _TSuperclass >
229 fpa::Base::MinimumSpanningTree< _TVertex, _TPath, _TSuperclass >::
230 MinimumSpanningTree( )
235 // -------------------------------------------------------------------------
236 template< class _TVertex, class _TPath, class _TSuperclass >
237 fpa::Base::MinimumSpanningTree< _TVertex, _TPath, _TSuperclass >::
238 ~MinimumSpanningTree( )
242 #endif // __fpa__Base__MinimumSpanningTree__hxx__