lib/cpm/Algorithms/Simplex/InternalForceFunction.hxx

   1 #ifndef __CPM__ALGORITHMS__SIMPLEX__INTERNALFORCEFUNCTION__HXX__
   2 #define __CPM__ALGORITHMS__SIMPLEX__INTERNALFORCEFUNCTION__HXX__
   3
   4 #include <cmath>
   5 #include <vnl/vnl_math.h>
   6
   7 // -------------------------------------------------------------------------
   8 template< class M >
   9 typename cpm::Algorithms::Simplex::InternalForceFunction< M >::
  10 TVector cpm::Algorithms::Simplex::InternalForceFunction< M >::
  11 Evaluate( const TPointId& pId ) const
  12 {
  13   typedef typename M::BarycenterVector TBarycenter;
  14
  15   static const TScalar _1_3 = TScalar( 1 ) / TScalar( 3 );
  16
  17   TVector f( TScalar( 0 ) );
  18   if( this->m_Mesh.IsNotNull( ) )
  19   {
  20     if( pId < this->m_Mesh->GetNumberOfPoints( ) )
  21     {
  22       TVector A, B, C, N;
  23       TBarycenter b;
  24       TScalar r, d, dp, phi;
  25       if(
  26         this->m_Mesh->
  27         ComputeSimplexParameters( pId, A, B, C, N, b, r, d, dp, phi )
  28         )
  29       {
  30         // Parallel force
  31         f  = A * ( _1_3 - b[ 0 ] );
  32         f += B * ( _1_3 - b[ 1 ] );
  33         f += C * ( _1_3 - b[ 2 ] );
  34
  35         // Desired phi
  36         TScalar dphi;
  37         switch( this->m_PhiConstraint )
  38         {
  39         case Self::C1:
  40           dphi = TScalar( 0 );
  41           break;
  42         case Self::C2:
  43           dphi = this->_ComputeMeanPhi( pId );
  44           break;
  45         case Self::C3:
  46           dphi = this->m_InitialPhiAngles[ pId ];
  47           break;
  48         case Self::C0:
  49         default:
  50           dphi = phi;
  51           break;
  52         } // hctiws
  53
  54         // Regularizing force
  55         f += N * ( Self::_L( r, dp, dphi ) - Self::_L( r, d, phi ) );
  56
  57       } // fi
  58
  59     } // fi
  60
  61   } // fi
  62   return( f );
  63 }
  64
  65 // -------------------------------------------------------------------------
  66 template< class M >
  67 void cpm::Algorithms::Simplex::InternalForceFunction< M >::
  68 SetMesh( const M* m )
  69 {
  70   typedef typename M::BarycenterVector TBarycenter;
  71
  72   if( this->m_Mesh != m && m != NULL )
  73   {
  74     this->m_InitialPhiAngles.clear( );
  75     for( TPointId i = 0; i < m->GetNumberOfPoints( ); ++i )
  76     {
  77       TVector A, B, C, N;
  78       TBarycenter e;
  79       TScalar a, b, c, phi = TScalar( 0 );
  80       m->ComputeSimplexParameters( i, A, B, C, N, e, a, b, c, phi );
  81       this->m_InitialPhiAngles.push_back( phi );
  82
  83     } // rof
  84
  85   } // fi
  86   this->Superclass::SetMesh( m );
  87 }
  88
  89 // -------------------------------------------------------------------------
  90 template< class M >
  91 void cpm::Algorithms::Simplex::InternalForceFunction< M >::
  92 SetPhiConstraintToC0( )
  93 {
  94   if( this->m_PhiConstraint != Self::C0 )
  95   {
  96     this->m_PhiConstraint = Self::C0;
  97     this->Modified( );
  98
  99   } // fi
 100 }
 101
 102 // -------------------------------------------------------------------------
 103 template< class M >
 104 void cpm::Algorithms::Simplex::InternalForceFunction< M >::
 105 SetPhiConstraintToC1( )
 106 {
 107   if( this->m_PhiConstraint != Self::C1 )
 108   {
 109     this->m_PhiConstraint = Self::C1;
 110     this->Modified( );
 111
 112   } // fi
 113 }
 114
 115 // -------------------------------------------------------------------------
 116 template< class M >
 117 void cpm::Algorithms::Simplex::InternalForceFunction< M >::
 118 SetPhiConstraintToC2( const unsigned int& neigh_order )
 119 {
 120   if( this->m_PhiConstraint != Self::C2 )
 121   {
 122     this->m_PhiConstraint = Self::C2;
 123     this->m_NeighborhoodOrder = neigh_order;
 124     this->Modified( );
 125
 126   } // fi
 127 }
 128
 129 // -------------------------------------------------------------------------
 130 template< class M >
 131 void cpm::Algorithms::Simplex::InternalForceFunction< M >::
 132 SetPhiConstraintToC3( )
 133 {
 134   if( this->m_PhiConstraint != Self::C3 )
 135   {
 136     this->m_PhiConstraint = Self::C3;
 137     this->Modified( );
 138
 139   } // fi
 140 }
 141
 142 // -------------------------------------------------------------------------
 143 template< class M >
 144 cpm::Algorithms::Simplex::InternalForceFunction< M >::
 145 InternalForceFunction( )
 146   : Superclass( ),
 147     m_PhiConstraint( Self::C0 ),
 148     m_NeighborhoodOrder( 1 )
 149 {
 150 }
 151
 152 // -------------------------------------------------------------------------
 153 template< class M >
 154 cpm::Algorithms::Simplex::InternalForceFunction< M >::
 155 ~InternalForceFunction( )
 156 {
 157 }
 158
 159 // -------------------------------------------------------------------------
 160 template< class M >
 161 typename cpm::Algorithms::Simplex::InternalForceFunction< M >::
 162 TScalar cpm::Algorithms::Simplex::InternalForceFunction< M >::
 163 _ComputeMeanPhi( const TPointId& pId ) const
 164 {
 165   // TODO: use this->m_NeighborhoodOrder
 166   return( TScalar( 0 ) );
 167 }
 168
 169 // -------------------------------------------------------------------------
 170 template< class M >
 171 typename cpm::Algorithms::Simplex::InternalForceFunction< M >::
 172 TScalar cpm::Algorithms::Simplex::InternalForceFunction< M >::
 173 _L( const TScalar& delta, const TScalar& d, const TScalar& phi )
 174 {
 175   static const double _pi2 = double( vnl_math::pi ) / double( 2 );
 176
 177   TScalar eps = TScalar( 1 );
 178   if( _pi2 < std::fabs( double( phi ) ) )
 179     eps = TScalar( -1 );
 180
 181   TScalar sphi = TScalar( std::sin( double( phi ) ) );
 182   TScalar cphi = TScalar( std::fabs( std::cos( double( phi ) ) ) );
 183
 184   TScalar den = delta * cphi;
 185   TScalar s = ( delta + ( d * sphi ) ) * ( delta - ( d * sphi ) );
 186   if( s < TScalar( 0 ) )
 187     return( TScalar( 0 ) );
 188
 189   den += eps * TScalar( std::sqrt( double( s ) ) );
 190   if( !( TScalar( 0 ) < den ) )
 191     return( TScalar( 0 ) );
 192
 193   TScalar num = eps * ( delta + d ) * ( delta - d ) * sphi;
 194   return( num / den );
 195 }
 196
 197 #endif // __CPM__ALGORITHMS__SIMPLEX__INTERNALFORCEFUNCTION__HXX__
 198
 199 // eof - $RCSfile$