1 #ifndef __FPA__BASE__MINIMUMSPANNINGTREE__HXX__
2 #define __FPA__BASE__MINIMUMSPANNINGTREE__HXX__
6 // -------------------------------------------------------------------------
7 template< class _TSuperclass, class _TVertex >
8 const unsigned long fpa::Base::
9 MinimumSpanningTree< _TSuperclass, _TVertex >::INF_VALUE =
10 std::numeric_limits< unsigned long >::max( );
12 // -------------------------------------------------------------------------
13 template< class _TSuperclass, class _TVertex >
14 void fpa::Base::MinimumSpanningTree< _TSuperclass, _TVertex >::
15 SetCollisions( const TCollisions& collisions )
17 // Prepare a front graph
18 this->m_Collisions = collisions;
19 unsigned long nSeeds = this->m_Collisions.size( );
20 _TMatrix dist( nSeeds, _TRow( nSeeds, Self::INF_VALUE ) );
21 this->m_FrontPaths = dist;
22 for( unsigned long i = 0; i < nSeeds; ++i )
24 for( unsigned long j = 0; j < nSeeds; ++j )
26 if( this->m_Collisions[ i ][ j ].second )
30 this->m_FrontPaths[ i ][ j ] = j;
31 this->m_FrontPaths[ j ][ i ] = i;
37 this->m_FrontPaths[ i ][ i ] = i;
41 // Use Floyd-Warshall to compute all possible paths
42 for( unsigned long k = 0; k < nSeeds; ++k )
44 for( unsigned long i = 0; i < nSeeds; ++i )
46 for( unsigned long j = 0; j < nSeeds; ++j )
48 if( ( dist[ i ][ k ] + dist[ k ][ j ] ) < dist[ i ][ j ] )
50 dist[ i ][ j ] = dist[ i ][ k ] + dist[ k ][ j ];
51 this->m_FrontPaths[ i ][ j ] = this->m_FrontPaths[ i ][ k ];
63 // -------------------------------------------------------------------------
64 template< class _TSuperclass, class _TVertex >
65 typename fpa::Base::MinimumSpanningTree< _TSuperclass, _TVertex >::
66 TVertices fpa::Base::MinimumSpanningTree< _TSuperclass, _TVertex >::
67 GetPath( const TVertex& a ) const
70 if( this->_HasVertex( a ) )
71 this->_Path( path, a );
75 // -------------------------------------------------------------------------
76 template< class _TSuperclass, class _TVertex >
77 typename fpa::Base::MinimumSpanningTree< _TSuperclass, _TVertex >::
78 TVertices fpa::Base::MinimumSpanningTree< _TSuperclass, _TVertex >::
79 GetPath( const TVertex& a, const TVertex& b ) const
84 if( !this->_HasVertex( a ) || !this->_HasVertex( b ) )
88 short fa = this->_FrontId( a );
89 short fb = this->_FrontId( b );
98 // Ignore common part: find common ancestor
99 auto raIt = ap.rbegin( );
100 auto rbIt = bp.rbegin( );
101 while( *raIt == *rbIt && raIt != ap.rend( ) && rbIt != bp.rend( ) )
107 if( raIt != ap.rbegin( ) ) --raIt;
108 if( rbIt != bp.rbegin( ) ) --rbIt;
111 for( auto iaIt = ap.begin( ); iaIt != ap.end( ) && *iaIt != *raIt; ++iaIt )
112 path.push_back( *iaIt );
115 for( ; rbIt != bp.rend( ); ++rbIt )
116 path.push_back( *rbIt );
120 // Use this->m_FrontPaths from Floyd-Warshall
121 if( this->m_FrontPaths[ fa ][ fb ] < Self::INF_VALUE )
123 // Compute front path
124 std::vector< long > fpath;
125 fpath.push_back( fa );
128 fa = this->m_FrontPaths[ fa ][ fb ];
129 fpath.push_back( fa );
133 // Continue only if both fronts are connected
134 unsigned int N = fpath.size( );
137 // First path: from start vertex to first collision
138 path = this->GetPath(
139 a, this->m_Collisions[ fpath[ 0 ] ][ fpath[ 1 ] ].first
142 // Intermediary paths
143 for( unsigned int i = 1; i < N - 1; ++i )
147 this->m_Collisions[ fpath[ i ] ][ fpath[ i - 1 ] ].first,
148 this->m_Collisions[ fpath[ i ] ][ fpath[ i + 1 ] ].first
150 path.insert( path.end( ), ipath.begin( ), ipath.end( ) );
154 // Final path: from last collision to end point
157 this->m_Collisions[ fpath[ N - 1 ] ][ fpath[ N - 2 ] ].first, b
159 path.insert( path.end( ), lpath.begin( ), lpath.end( ) );
169 // -------------------------------------------------------------------------
170 template< class _TSuperclass, class _TVertex >
171 typename fpa::Base::MinimumSpanningTree< _TSuperclass, _TVertex >::
172 TPoints fpa::Base::MinimumSpanningTree< _TSuperclass, _TVertex >::
173 GetEuclideanPath( const TVertex& a ) const
179 // -------------------------------------------------------------------------
180 template< class _TSuperclass, class _TVertex >
181 typename fpa::Base::MinimumSpanningTree< _TSuperclass, _TVertex >::
182 TPoints fpa::Base::MinimumSpanningTree< _TSuperclass, _TVertex >::
183 GetEuclideanPath( const TVertex& a, const TVertex& b ) const
189 // -------------------------------------------------------------------------
190 template< class _TSuperclass, class _TVertex >
191 bool fpa::Base::MinimumSpanningTree< _TSuperclass, _TVertex >::
192 IsDefinedInEuclideanSpace( ) const
197 // -------------------------------------------------------------------------
198 template< class _TSuperclass, class _TVertex >
199 void fpa::Base::MinimumSpanningTree< _TSuperclass, _TVertex >::
201 const TVertex& v, const TVertex& p,
202 const short& fid, const double& cost
205 typedef typename TNodeQueue::value_type _TNodeQueueValue;
206 if( this->m_FillNodeQueue )
207 this->m_NodeQueue.insert( _TNodeQueueValue( cost, v ) );
210 // -------------------------------------------------------------------------
211 template< class _TSuperclass, class _TVertex >
212 void fpa::Base::MinimumSpanningTree< _TSuperclass, _TVertex >::
215 this->m_NodeQueue.clear( );
218 // -------------------------------------------------------------------------
219 template< class _TSuperclass, class _TVertex >
220 fpa::Base::MinimumSpanningTree< _TSuperclass, _TVertex >::
221 MinimumSpanningTree( )
223 m_FillNodeQueue( false )
227 // -------------------------------------------------------------------------
228 template< class _TSuperclass, class _TVertex >
229 fpa::Base::MinimumSpanningTree< _TSuperclass, _TVertex >::
230 ~MinimumSpanningTree( )
234 #endif // __FPA__BASE__MINIMUMSPANNINGTREE__HXX__