lib/fpa/Base/FastMarching.hxx

   1 #ifndef __fpa__Base__FastMarching__hxx__
   2 #define __fpa__Base__FastMarching__hxx__
   3
   4 // -------------------------------------------------------------------------
   5 template< class _TSuperclass >
   6 fpa::Base::FastMarching< _TSuperclass >::
   7 FastMarching( )
   8   : Superclass( )
   9 {
  10 }
  11
  12 // -------------------------------------------------------------------------
  13 template< class _TSuperclass >
  14 fpa::Base::FastMarching< _TSuperclass >::
  15 ~FastMarching( )
  16 {
  17 }
  18
  19 // -------------------------------------------------------------------------
  20 template< class _TSuperclass >
  21 bool fpa::Base::FastMarching< _TSuperclass >::
  22 _UpdateValue( _TQueueNode& v, const _TQueueNode& p )
  23 {
  24   // Compute quadratic coefficients
  25   double a = double( 0 );
  26   double b = double( 0 );
  27   double c = double( 0 );
  28   TFastMarchingNeighborhood neighs = this->_FastMarchingNeighbors( v.Vertex );
  29   for( unsigned int i = 0; i < neighs.size( ); i += 2 )
  30   {
  31     double tn = double( this->_GetResult( neighs[ i ].first ) );
  32     double tp = double( this->_GetResult( neighs[ i + 1 ].first ) );
  33     double dd = double( 0 );
  34     double td = this->m_InitResult;
  35     if( tn < tp )
  36     {
  37       td = tn;
  38       dd = neighs[ i ].second;
  39     }
  40     else
  41     {
  42       td = tp;
  43       dd = neighs[ i + 1 ].second;
  44
  45     } // fi
  46
  47     if( td < double( this->m_InitResult ) )
  48     {
  49       dd = double( 1 ) / ( dd * dd );
  50       a += dd;
  51       b += td * dd;
  52       c += td * td * dd;
  53
  54     } // fi
  55
  56   } // rof
  57   double F = double( this->_GetInputValue( v, p ) );
  58   c -= double( 1 ) / ( F * F );
  59
  60   // Solve quadratic equation
  61   double d = ( b * b ) - ( a * c );
  62   if( d >= double( 0 ) )
  63   {
  64     d = std::sqrt( d );
  65     b *= double( -1 );
  66     double s1 = std::fabs( ( b + d ) / a );
  67     double s2 = std::fabs( ( b - d ) / a );
  68     v.Result = TOutput( ( s2 < s1 )? s1: s2 );
  69     if( v.Result < this->_GetResult( v.Vertex ) )
  70       this->_UpdateResult( v );
  71   }
  72   else
  73   {
  74     std::cout << std::endl << "-- FM --> " << v.Vertex << " " << d << std::endl;
  75     std::exit( 1 );
  76   }
  77   return( true );
  78 }
  79
  80 #endif // __fpa__Base__FastMarching__hxx__
  81
  82 // eof - $RCSfile$