5 #include <vnl/vnl_math.h>
6 #include "FourierSeries.h"
8 // -------------------------------------------------------------------------
9 template< class S, unsigned int D >
10 FourierSeries< S, D >::
11 FourierSeries( unsigned int q )
12 : m_RealAxis( D - 2 ),
15 this->resize( ( q << 1 ) + 1, TComplex( S( 0 ), S( 0 ) ) );
18 // -------------------------------------------------------------------------
19 template< class S, unsigned int D >
20 FourierSeries< S, D >::
25 // -------------------------------------------------------------------------
26 template< class S, unsigned int D >
27 const unsigned int& FourierSeries< S, D >::
30 return( this->m_RealAxis );
33 // -------------------------------------------------------------------------
34 template< class S, unsigned int D >
35 const unsigned int& FourierSeries< S, D >::
38 return( this->m_ImagAxis );
41 // -------------------------------------------------------------------------
42 template< class S, unsigned int D >
43 void FourierSeries< S, D >::
44 SetRealAxis( const unsigned int& a )
49 // -------------------------------------------------------------------------
50 template< class S, unsigned int D >
51 void FourierSeries< S, D >::
52 SetImagAxis( const unsigned int& a )
57 // -------------------------------------------------------------------------
58 template< class S, unsigned int D >
59 S FourierSeries< S, D >::
63 int q = this->GetNumberOfHarmonics( );
64 typename Self::const_iterator i = this->begin( );
65 for( int l = -q; i != this->end( ); ++i, ++l )
66 a += S( l ) * std::norm( *i );
67 return( S( vnl_math::pi ) * a );
70 // -------------------------------------------------------------------------
71 template< class S, unsigned int D >
72 S FourierSeries< S, D >::
73 GetCurvature( const S& w ) const
75 TComplex d1 = this->_Z( w, 1 );
76 TComplex d2 = this->_Z( w, 2 );
79 return( std::imag( std::conj( d1 ) * d2 ) / ( d * d * d ) );
84 // -------------------------------------------------------------------------
85 template< class S, unsigned int D >
86 void FourierSeries< S, D >::
87 SetOrdering( bool cw )
89 // Check if the area sign is different from the desired orientation
90 bool ap = ( S( 0 ) < this->GetArea( ) );
91 if( ( !ap && cw ) || ( ap && !cw ) ) // XOR
92 this->InvertOrdering( );
95 // -------------------------------------------------------------------------
96 template< class S, unsigned int D >
97 void FourierSeries< S, D >::
100 int q = this->GetNumberOfHarmonics( );
101 for( int l = 1; l <= q; l++ )
103 TComplex z = ( *this )[ l ];
104 ( *this )[ l ] = ( *this )[ -l ];
110 // -------------------------------------------------------------------------
111 template< class S, unsigned int D >
112 void FourierSeries< S, D >::
113 SetOrderingToClockWise( )
115 this->SetOrdering( false );
118 // -------------------------------------------------------------------------
119 template< class S, unsigned int D >
120 void FourierSeries< S, D >::
121 SetOrderingToCounterClockWise( )
123 this->SetOrdering( true );
126 // -------------------------------------------------------------------------
127 template< class S, unsigned int D >
128 int FourierSeries< S, D >::
129 GetNumberOfHarmonics( ) const
131 return( ( this->size( ) - 1 ) >> 1 );
134 // -------------------------------------------------------------------------
135 template< class S, unsigned int D >
136 void FourierSeries< S, D >::
137 SetNumberOfHarmonics( const int& q )
139 int diff_q = this->GetNumberOfHarmonics( ) - q;
151 this->push_front( TComplex( S( 0 ), S( 0 ) ) );
152 this->push_back( TComplex( S( 0 ), S( 0 ) ) );
160 // -------------------------------------------------------------------------
161 template< class S, unsigned int D >
162 typename FourierSeries< S, D >::
163 TComplex& FourierSeries< S, D >::
164 operator[]( const int& l )
166 static TComplex _zero;
167 int idx = ( ( this->size( ) - 1 ) >> 1 ) + l;
168 if( idx < 0 || idx >= this->size( ) )
170 _zero = TComplex( S( 0 ) );
174 return( this->Superclass::operator[]( idx ) );
177 // -------------------------------------------------------------------------
178 template< class S, unsigned int D >
179 const typename FourierSeries< S, D >::
180 TComplex& FourierSeries< S, D >::
181 operator[]( const int& l ) const
183 static const TComplex _zero( S( 0 ) );
184 int idx = ( ( this->size( ) - 1 ) >> 1 ) + l;
185 if( idx < 0 || idx >= this->size( ) )
188 return( this->Superclass::operator[]( idx ) );
191 // -------------------------------------------------------------------------
192 template< class S, unsigned int D >
193 typename FourierSeries< S, D >::
194 TPoint FourierSeries< S, D >::
195 operator( )( const S& w ) const
197 TComplex z = this->_Z( w, 0 );
200 p[ this->m_RealAxis ] = z.real( );
201 p[ this->m_ImagAxis ] = z.imag( );
205 // -------------------------------------------------------------------------
206 template< class S, unsigned int D >
207 typename FourierSeries< S, D >::
208 TVector FourierSeries< S, D >::
209 operator( )( const S& w, const unsigned int& n ) const
211 TComplex z = this->_Z( w, n );
213 v[ this->m_RealAxis ] = z.real( );
214 v[ this->m_ImagAxis ] = z.imag( );
218 // -------------------------------------------------------------------------
219 template< class S, unsigned int D >
220 typename FourierSeries< S, D >::
221 Self FourierSeries< S, D >::
222 operator+( const Self& o ) const
224 int q1 = o.GetNumberOfHarmonics( );
225 int q2 = this->GetNumberOfHarmonics( );
226 int q = ( q2 < q1 )? q1: q2;
229 res.SetNumberOfHarmonics( q );
230 for( int l = -q; l <= q; ++l )
231 res[ l ] = ( *this )[ l ] + o[ l ];
236 // -------------------------------------------------------------------------
237 template< class S, unsigned int D >
238 typename FourierSeries< S, D >::
239 Self& FourierSeries< S, D >::
240 operator+=( const Self& o )
242 int q1 = o.GetNumberOfHarmonics( );
243 int q2 = this->GetNumberOfHarmonics( );
244 int q = ( q2 < q1 )? q1: q2;
246 this->SetNumberOfHarmonics( q );
247 for( int l = -q; l <= q; ++l )
248 ( *this )[ l ] += o[ l ];
253 // -------------------------------------------------------------------------
254 template< class S, unsigned int D >
255 typename FourierSeries< S, D >::
256 Self FourierSeries< S, D >::
257 operator-( const Self& o ) const
259 int q1 = o.GetNumberOfHarmonics( );
260 int q2 = this->GetNumberOfHarmonics( );
261 int q = ( q2 < q1 )? q1: q2;
264 res.SetNumberOfHarmonics( q );
265 for( int l = -q; l <= q; ++l )
266 res[ l ] = ( *this )[ l ] - o[ l ];
271 // -------------------------------------------------------------------------
272 template< class S, unsigned int D >
273 typename FourierSeries< S, D >::
274 Self& FourierSeries< S, D >::
275 operator-=( const Self& o )
277 int q1 = o.GetNumberOfHarmonics( );
278 int q2 = this->GetNumberOfHarmonics( );
279 int q = ( q2 < q1 )? q1: q2;
281 this->SetNumberOfHarmonics( q );
282 for( int l = -q; l <= q; ++l )
283 ( *this )[ l ] -= o[ l ];
288 // -------------------------------------------------------------------------
289 template< class S, unsigned int D >
290 typename FourierSeries< S, D >::
291 Self FourierSeries< S, D >::
292 operator+( const TComplex& translation ) const
295 res[ 0 ] += translation;
299 // -------------------------------------------------------------------------
300 template< class S, unsigned int D >
301 typename FourierSeries< S, D >::
302 Self& FourierSeries< S, D >::
303 operator+=( const TComplex& translation )
305 ( *this )[ 0 ] += translation;
309 // -------------------------------------------------------------------------
310 template< class S, unsigned int D >
311 typename FourierSeries< S, D >::
312 Self FourierSeries< S, D >::
313 operator-( const TComplex& translation ) const
316 res[ 0 ] -= translation;
320 // -------------------------------------------------------------------------
321 template< class S, unsigned int D >
322 typename FourierSeries< S, D >::
323 Self& FourierSeries< S, D >::
324 operator-=( const TComplex& translation )
326 ( *this )[ 0 ] -= translation;
330 // -------------------------------------------------------------------------
331 template< class S, unsigned int D >
332 typename FourierSeries< S, D >::
333 Self FourierSeries< S, D >::
334 operator+( const TVector& translation ) const
338 TComplex( translation[ this->m_RealAxis ], translation[ this->m_ImagAxis ] )
342 // -------------------------------------------------------------------------
343 template< class S, unsigned int D >
344 typename FourierSeries< S, D >::
345 Self& FourierSeries< S, D >::
346 operator+=( const TVector& translation )
349 TComplex( translation[ this->m_RealAxis ], translation[ this->m_ImagAxis ] );
353 // -------------------------------------------------------------------------
354 template< class S, unsigned int D >
355 typename FourierSeries< S, D >::
356 Self FourierSeries< S, D >::
357 operator-( const TVector& translation ) const
361 TComplex( translation[ this->m_RealAxis ], translation[ this->m_ImagAxis ] )
365 // -------------------------------------------------------------------------
366 template< class S, unsigned int D >
367 typename FourierSeries< S, D >::
368 Self& FourierSeries< S, D >::
369 operator-=( const TVector& translation )
372 TComplex( translation[ this->m_RealAxis ], translation[ this->m_ImagAxis ] );
376 // -------------------------------------------------------------------------
377 template< class S, unsigned int D >
378 typename FourierSeries< S, D >::
379 TPoint FourierSeries< S, D >::
382 TComplex z = ( *this )[ 0 ];
385 p[ this->m_RealAxis ] = z.real( );
386 p[ this->m_ImagAxis ] = z.imag( );
390 // -------------------------------------------------------------------------
391 template< class S, unsigned int D >
392 typename FourierSeries< S, D >::
393 Self FourierSeries< S, D >::
394 operator*( const TComplex& scale_or_rotation ) const
398 for( typename Self::const_iterator i = this->begin( ); i != this->end( ); ++i )
399 res.push_back( *i * scale_or_rotation );
403 // -------------------------------------------------------------------------
404 template< class S, unsigned int D >
405 typename FourierSeries< S, D >::
406 Self& FourierSeries< S, D >::
407 operator*=( const TComplex& scale_or_rotation )
409 for( typename Self::iterator i = this->begin( ); i != this->end( ); ++i )
410 *i *= scale_or_rotation;
414 // -------------------------------------------------------------------------
415 template< class S, unsigned int D >
416 typename FourierSeries< S, D >::
417 Self FourierSeries< S, D >::
418 operator/( const S& scale ) const
422 for( typename Self::const_iterator i = this->begin( ); i != this->end( ); ++i )
423 res.push_back( *i / scale );
427 // -------------------------------------------------------------------------
428 template< class S, unsigned int D >
429 typename FourierSeries< S, D >::
430 Self& FourierSeries< S, D >::
431 operator/=( const S& scale )
433 for( typename Self::iterator i = this->begin( ); i != this->end( ); ++i )
438 // -------------------------------------------------------------------------
439 template< class S, unsigned int D >
440 typename FourierSeries< S, D >::
441 Self FourierSeries< S, D >::
442 operator&( const S& phase ) const
446 int q = this->GetNumberOfHarmonics( );
447 typename Self::const_iterator i = this->begin( );
448 for( int l = -q; i != this->end( ); ++i, ++l )
449 res.push_back( *i * std::polar( S( 1 ), S( l ) * phase ) );
453 // -------------------------------------------------------------------------
454 template< class S, unsigned int D >
455 typename FourierSeries< S, D >::
456 Self& FourierSeries< S, D >::
457 operator&=( const S& phase )
459 int q = this->GetNumberOfHarmonics( );
460 typename Self::iterator i = this->begin( );
461 for( int l = -q; i != this->end( ); ++i, ++l )
462 *i *= std::polar( S( 1 ), S( l ) * phase );
466 // -------------------------------------------------------------------------
467 template< class S, unsigned int D >
468 S FourierSeries< S, D >::
469 GetPhase( const S& w ) const
471 // Some constant values
472 static const S _0 = S( 0 );
473 static const S _1 = S( 1 );
474 static const S _2 = S( 2 );
475 static const S _2pi = _2 * S( vnl_math::pi );
476 static const S _epsilon = S( 1e-14 );
477 static const S _tolerance = S( 1e-10 );
478 static const unsigned int _samples = 100;
479 static const unsigned int _maxIterations = 1000;
480 static const S _angleOff = _2pi / S( _samples );
483 int q = int( this->GetNumberOfHarmonics( ) );
487 // Get a centered copy
488 Self contour = *this;
489 contour[ 0 ] = TComplex( _0, _0 );
491 // Compute maximum coefficient norm
493 for( int l = 1; l <= q; ++l )
495 S np = std::abs( contour[ l ] );
496 S nn = std::abs( contour[ -l ] );
497 S n = ( np < nn )? nn: np;
503 // Rotate to desired phase
504 contour *= std::polar( _1, -w );
506 // Try to normalize contour: if not, malformed contour!
511 // 1. Approximate initial guess by a simple dichotomy
512 std::vector< std::pair< S, S > > function;
513 S minA = std::numeric_limits< S >::max( );
514 unsigned int minIdx = -1;
516 // 1.1. Roughly sample phases
517 for( unsigned int s = 0; s < _samples; ++s )
519 S w = S( s ) * _angleOff;
520 S a = std::arg( contour._Z( w, 0 ) );
521 function.push_back( std::pair< S, S >( w, a ) );
531 // 1.2. Get zero cuts by zero crossing analysis and keep the farthest
532 // point in the real axis (ie. the last data in "cuts")
534 std::map< S, unsigned int > cuts;
535 for( unsigned int s = 0; s < _samples; ++s )
537 unsigned int i = ( s + minIdx ) % _samples;
538 if( function[ i ].second * prevA < _0 )
539 cuts[ std::real( contour._Z( function[ i ].first, 0 ) ) ] = i;
540 prevA = function[ i ].second;
544 // 1.3. Get initial guess
546 if( cuts.size( ) > 0 )
548 unsigned int i = cuts.rbegin( )->second;
550 i = function.size( );
551 w0 = function[ i - 1 ].first;
555 // 2. Refine by Newthon-Raphson
558 while( i < _maxIterations && !stop )
560 TComplex c = contour._Z( w0, 0 );
561 S d = std::imag( c * std::conj( contour._Z( w0, 1 ) ) );
564 if( !( std::fabs( d ) < _epsilon ) )
566 S w1 = w0 + ( std::arg( c ) * ( std::norm( c ) / d ) );
567 if( ( std::fabs( w1 - w0 ) / std::fabs( w1 ) ) < _tolerance )
583 // -------------------------------------------------------------------------
584 template< class S, unsigned int D >
585 void FourierSeries< S, D >::
586 Sample( std::vector< TPoint >& p, const unsigned long& s ) const
588 static const S _2pi = S( 2 ) * S( vnl_math::pi );
589 S off = _2pi / S( s - 1 );
590 for( unsigned int w = 0; w < s; ++w )
591 p.push_back( ( *this )( S( w ) * off ) );
594 // -------------------------------------------------------------------------
595 template< class S, unsigned int D >
596 void FourierSeries< S, D >::
597 GetEllipse( int l, S& a, S& b, S& t, S& p ) const
599 a = b = t = p = S( 0 );
600 if( l == 0 || l > this->GetNumberOfHarmonics( ) )
603 TComplex zp = ( *this )[ l ];
604 TComplex zn = ( *this )[ -l ];
605 S np = std::abs( zp );
606 S nn = std::abs( zn );
612 // Rotation and phase
613 if( np > S( 0 ) && nn > S( 0 ) )
617 t = std::real( std::log( zp * zn ) / TComplex( S( 0 ), S( 2 ) ) );
618 p = std::real( std::log( zp / zn ) / TComplex( S( 0 ), S( 2 * l ) ) );
623 p = ( np > S( 0 ) )? std::arg( zp ): std::arg( zn );
628 // -------------------------------------------------------------------------
629 template< class S, unsigned int D >
630 void FourierSeries< S, D >::
631 SetEllipse( int l, S& a, S& b, S& t, S& p )
633 S nzp = ( a + b ) / S( 2 );
634 S nzn = ( a - b ) / S( 2 );
635 TComplex et = std::polar( S( 1 ), t );
636 TComplex ep = std::polar( S( 1 ), S( l ) * p );
637 ( *this )[ l ] = et * ep * nzp;
638 ( *this )[ -l ] = et * std::conj( ep ) * nzn;
641 // -------------------------------------------------------------------------
642 template< class S, unsigned int D >
643 typename FourierSeries< S, D >::
644 TComplex FourierSeries< S, D >::
645 _Z( const S& w, const unsigned int& n ) const
647 static const S _0 = S( 0 );
648 static const S _1 = S( 1 );
651 TComplex z( _0, _0 );
652 int q = this->GetNumberOfHarmonics( );
653 for( int l = -q; l <= q; ++l )
655 TComplex v = ( *this )[ l ] * std::polar( _1, w * S( l ) );
657 v *= std::pow( TComplex( _0, S( l ) ), _n );
664 // -------------------------------------------------------------------------
665 template< class S, unsigned int D >
666 void FourierSeries< S, D >::
667 _DFT( const std::vector< TComplex >& p, unsigned int q )
669 static const S _2pi = S( 2 ) * S( vnl_math::pi );
671 this->SetNumberOfHarmonics( q );
676 std::vector< TComplex > dft;
677 for( long m = 0; m < p.size( ); ++m )
679 TComplex z( S( 0 ), S( 0 ) );
680 for( long k = 0; k < p.size( ); ++k )
683 std::polar( S( 1 ), -( _2pi * S( m * k ) ) / S( p.size( ) ) );
689 ( *this )[ 0 ] = dft[ 0 ];
690 for( int l = 1; l <= q; ++l )
692 ( *this )[ l ] = dft[ l ];
693 ( *this )[ -l ] = dft[ p.size( ) - l ];
699 this->SetNumberOfHarmonics( q );
701 // Compute number of calculations K: cut harmonics to q
702 long N = long( p.size( ) );
705 long K = ( long( q ) << 1 ) + 1;
710 for( long l = -K; l <= K; ++l )
712 S k = ( ( S( l ) + S( ( l < 0 )? N: 0 ) ) * _2pi ) / S( N );
713 ( *this )[ l ] = TComplex( S( 0 ), S( 0 ) );
714 for( long n = 0; n < N; ++n )
715 ( *this )[ l ] += p[ n ] * std::polar( S( 1 ), k * S( -n ) );
716 ( *this )[ l ] /= S( N );
722 // -------------------------------------------------------------------------
723 template class FourierSeries< float, 2 >;
724 template class FourierSeries< float, 3 >;
725 template class FourierSeries< double, 2 >;
726 template class FourierSeries< double, 3 >;