1 // -------------------------------------------------------------------------
2 // @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
3 // -------------------------------------------------------------------------
5 #ifndef __CPPLUGINS__EXTENSIONS__ALGORITHMS__BEZIERCURVEFUNCTION__HXX__
6 #define __CPPLUGINS__EXTENSIONS__ALGORITHMS__BEZIERCURVEFUNCTION__HXX__
8 // -------------------------------------------------------------------------
10 void cpPlugins::Extensions::Algorithms::BezierCurveFunction< V >::
11 AddPoint( const TVector& v )
13 this->m_Vectors.push_back( v );
14 this->m_DerivativeUpdated = false;
18 // -------------------------------------------------------------------------
20 unsigned int cpPlugins::Extensions::Algorithms::BezierCurveFunction< V >::
21 GetNumberOfPoints( ) const
23 return( this->m_Vectors.size( ) );
26 // -------------------------------------------------------------------------
28 typename cpPlugins::Extensions::Algorithms::BezierCurveFunction< V >::
29 TVector cpPlugins::Extensions::Algorithms::BezierCurveFunction< V >::
30 Evaluate( const TScalar& u ) const
32 TVectorsContainer Q = this->m_Vectors;
33 unsigned int n = Q.size( );
34 TScalar _1u = TScalar( 1 ) - u;
36 for( unsigned int k = 1; k < n; k++ )
38 // CM Fixed a bug appearing under Windows : changed the stopping condition from <= to <.
39 // Otherwise, on the last step, an element out of the range of vector Q is accessed (Q[ i + 1 ])...
40 for( unsigned int i = 0; i < n - k; i++ )
41 Q[ i ] = ( Q[ i ] * _1u ) + ( Q[ i + 1 ] * u );
47 // -------------------------------------------------------------------------
49 typename cpPlugins::Extensions::Algorithms::BezierCurveFunction< V >::
50 TFrame cpPlugins::Extensions::Algorithms::BezierCurveFunction< V >::
51 EvaluateFrenetFrame( const TScalar& u ) const
54 fr.Fill( TScalar( 0 ) );
55 if( TVector::Dimension == 2 )
57 this->_UpdateDerivative( );
58 this->m_Derivative->_UpdateDerivative( );
60 TVector vT = this->m_Derivative->Evaluate( u );
61 TScalar nvT = vT.GetNorm( );
62 if( TScalar( 0 ) < nvT )
65 fr[ 0 ][ 0 ] = vT[ 0 ];
66 fr[ 1 ][ 0 ] = vT[ 1 ];
68 fr[ 0 ][ 1 ] = -vT[ 1 ];
69 fr[ 1 ][ 1 ] = vT[ 0 ];
74 if( TVector::Dimension == 3 )
76 this->_UpdateDerivative( );
77 this->m_Derivative->_UpdateDerivative( );
79 TVector vT = this->m_Derivative->Evaluate( u );
80 TVector vN = this->m_Derivative->m_Derivative->Evaluate( u );
81 TScalar nvT = vT.GetNorm( );
82 TScalar nvN = vN.GetNorm( );
84 if( nvT > TScalar( 0 ) && nvN > TScalar( 0 ) )
90 vB[ 0 ] = ( vT[ 1 ] * vN[ 2 ] ) - ( vT[ 2 ] * vN[ 1 ] );
91 vB[ 1 ] = ( vT[ 2 ] * vN[ 0 ] ) - ( vT[ 0 ] * vN[ 2 ] );
92 vB[ 2 ] = ( vT[ 0 ] * vN[ 1 ] ) - ( vT[ 1 ] * vN[ 0 ] );
94 TScalar nvB = vB.GetNorm( );
95 if( nvB > TScalar( 0 ) )
99 // WARNING: Paranoiac test
100 vN[ 0 ] = ( vB[ 1 ] * vT[ 2 ] ) - ( vB[ 2 ] * vT[ 1 ] );
101 vN[ 1 ] = ( vB[ 2 ] * vT[ 0 ] ) - ( vB[ 0 ] * vT[ 2 ] );
102 vN[ 2 ] = ( vB[ 0 ] * vT[ 1 ] ) - ( vB[ 1 ] * vT[ 0 ] );
104 for( unsigned int d = 0; d < 3; d++ )
106 fr[ d ][ 0 ] = vT[ d ];
107 fr[ d ][ 1 ] = vN[ d ];
108 fr[ d ][ 2 ] = vB[ d ];
114 // WARNING: Trick to avoid numerical instabilities
115 // in straight lines.
116 typedef itk::Vector< TScalar, 3 > _TVector3;
117 typedef ext::VectorToFrameFunction< _TVector3 > _TFunction;
124 typename _TFunction::Pointer fun = _TFunction::New( );
125 typename _TFunction::TFrame ffr = fun->Evaluate( vT3 );
127 fr[ 0 ][ 0 ] = ffr[ 0 ][ 0 ];
128 fr[ 0 ][ 1 ] = ffr[ 0 ][ 1 ];
129 fr[ 0 ][ 2 ] = ffr[ 0 ][ 2 ];
131 fr[ 1 ][ 0 ] = ffr[ 1 ][ 0 ];
132 fr[ 1 ][ 1 ] = ffr[ 1 ][ 1 ];
133 fr[ 1 ][ 2 ] = ffr[ 1 ][ 2 ];
135 fr[ 2 ][ 0 ] = ffr[ 2 ][ 0 ];
136 fr[ 2 ][ 1 ] = ffr[ 2 ][ 1 ];
137 fr[ 2 ][ 2 ] = ffr[ 2 ][ 2 ];
148 // -------------------------------------------------------------------------
150 typename cpPlugins::Extensions::Algorithms::BezierCurveFunction< V >::
151 TScalar cpPlugins::Extensions::Algorithms::BezierCurveFunction< V >::
152 EvaluateLength( ) const
154 unsigned int n = this->GetNumberOfPoints( ) << 1;
155 TScalar d = TScalar( 0 );
156 TVector v0 = this->Evaluate( 0 );
157 for( unsigned int i = 1; i < n; i++ )
159 TVector v1 = this->Evaluate( TScalar( i ) / TScalar( n - 1 ) );
160 d += ( v1 - v0 ).GetNorm( );
167 // -------------------------------------------------------------------------
169 cpPlugins::Extensions::Algorithms::BezierCurveFunction< V >::
170 BezierCurveFunction( )
172 m_DerivativeUpdated( false )
176 // -------------------------------------------------------------------------
178 void cpPlugins::Extensions::Algorithms::BezierCurveFunction< V >::
179 _UpdateDerivative( ) const
181 if( this->m_DerivativeUpdated )
184 this->m_Derivative = Self::New( );
185 unsigned int n = this->m_Vectors.size( ) - 1;
186 for( unsigned int i = 0; i < n; i++ )
187 this->m_Derivative->AddPoint(
188 TScalar( n ) * ( this->m_Vectors[ i + 1 ] - this->m_Vectors[ i ] )
192 #endif // __CPPLUGINS__EXTENSIONS__ALGORITHMS__BEZIERCURVEFUNCTION__HXX__