lib/cpExtensions/Algorithms/InertiaTensorFunction.h

   1 // -------------------------------------------------------------------------
   2 // @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
   3 // -------------------------------------------------------------------------
   4
   5 #ifndef __CPEXTENSIONS__ALGORITHMS__INERTIATENSORFUNCTION__H__
   6 #define __CPEXTENSIONS__ALGORITHMS__INERTIATENSORFUNCTION__H__
   7
   8 #include <itkObject.h>
   9
  10 #include <itkObject.h>
  11 #include <itkObjectFactory.h>
  12 #include <itkMatrix.h>
  13 #include <itkPoint.h>
  14
  15
  16 #include <cmath>
  17 #include <itkSymmetricEigenAnalysis.h>
  18
  19
  20 namespace cpExtensions
  21 {
  22   namespace Algorithms
  23   {
  24     /**
  25      */
  26     template< class S, unsigned int D >
  27     class InertiaTensorFunction
  28       : public itk::Object
  29     {
  30     public:
  31       // Standard itk types
  32       typedef InertiaTensorFunction           Self;
  33       typedef itk::Object                     Superclass;
  34       typedef itk::SmartPointer< Self >       Pointer;
  35       typedef itk::SmartPointer< const Self > ConstPointer;
  36
  37       typedef S                           TScalar;
  38       typedef itk::Matrix< S, D, D >      TMatrix;
  39       typedef itk::Point< S, D >          TPoint;
  40       typedef typename TPoint::VectorType TVector;
  41
  42     protected:
  43       typedef TMatrix _TInternalMatrix;
  44       typedef itk::Matrix< S, D, 1 > _TInternalVector;
  45
  46     public:
  47       itkNewMacro( Self );
  48       itkTypeMacro( InertiaTensorFunction, itkObject );
  49
  50     public:
  51       inline void Reset( )
  52         {
  53           this->m_M = S( 0 );
  54           this->m_MPP = S( 0 );
  55           this->m_MP.Fill( S( 0 ) );
  56           this->m_MPPT.Fill( S( 0 ) );
  57           this->Modified( );
  58         }
  59       inline void AddMass( const TPoint& pnt, const S& mass = S( 1 ) )
  60         {
  61           this->AddMass( pnt.GetVectorFromOrigin( ), mass );
  62         }
  63       inline void AddMass( const S* data, const S& mass = S( 1 ) )
  64         {
  65           this->AddMass( TVector( data ), mass );
  66         }
  67       inline void AddMass( const TVector& vec, const S& mass = S( 1 ) )
  68         {
  69           this->m_M += mass;
  70           this->m_MPP += mass * ( vec * vec );
  71
  72           _TInternalVector mp;
  73           for( unsigned int d = 0; d < D; ++d )
  74           {
  75             this->m_MP[ d ][ 0 ] += mass * vec[ d ];
  76             mp[ d ][ 0 ] = vec[ d ];
  77
  78           } // rof
  79           this->m_MPPT +=
  80             _TInternalMatrix( mp.GetVnlMatrix( ) * mp.GetTranspose( ) ) *
  81             mass;
  82         }
  83
  84       inline S GetMass( ) const
  85         {
  86           return( this->m_M );
  87         }
  88
  89       inline TMatrix GetInertia( ) const
  90         {
  91           TMatrix I;
  92           if( S( 0 ) < this->m_M )
  93           {
  94             I.SetIdentity( );
  95             I *= this->m_MPP - ( ( this->m_MP.GetTranspose( ) * this->m_MP.GetVnlMatrix( ) )[ 0 ][ 0 ] / this->m_M );
  96             I -= this->m_MPPT;
  97             I += TMatrix( this->m_MP.GetVnlMatrix( ) * this->m_MP.GetTranspose( ) ) / this->m_M;
  98           }
  99           else
 100             I.Fill( S( 0 ) );
 101           return( I );
 102         }
 103
 104       inline TVector GetCenterOfGravity( ) const
 105         {
 106           TVector cog;
 107           if( S( 0 ) < this->m_M )
 108           {
 109             for( unsigned int d = 0; d < D; ++d )
 110               cog[ d ] = this->m_MP[ d ][ 0 ] / this->m_M;
 111           }
 112           else
 113             cog.Fill( S( 0 ) );
 114           return( cog );
 115         }
 116
 117       inline void GetEigenAnalysis( TMatrix& pm, TVector& pv, TVector& r ) const
 118         {
 119           TMatrix I = this->GetInertia( );
 120
 121           itk::SymmetricEigenAnalysis< TMatrix, TVector, TMatrix > eigen;
 122           eigen.SetDimension( D );
 123           eigen.SetOrderEigenMagnitudes( true );
 124           eigen.SetOrderEigenValues( 1 );
 125           eigen.ComputeEigenValuesAndVectors( I, pv, pm );
 126           pm = TMatrix( pm.GetTranspose( ) );
 127           S det = vnl_determinant( pm.GetVnlMatrix( ) );
 128           for( unsigned int d = 0; d < D; ++d )
 129             pm[ d ][ D - 1 ] *= det;
 130
 131           if( D == 2 )
 132           {
 133             S coeff = S( 4 ) / this->m_M;
 134             r[ 0 ] = std::sqrt( std::fabs( coeff * pv[ 1 ] ) );
 135             r[ 1 ] = std::sqrt( std::fabs( coeff * pv[ 0 ] ) );
 136           }
 137           else if( D == 3 )
 138           {
 139             S coeff = S( 2.5 ) / this->m_M;
 140             r[ 0 ] = std::sqrt( std::fabs( coeff * ( pv[ 1 ] + pv[ 2 ] - pv[ 0 ] ) ) );
 141             r[ 1 ] = std::sqrt( std::fabs( coeff * ( pv[ 0 ] + pv[ 2 ] - pv[ 1 ] ) ) );
 142             r[ 2 ] = std::sqrt( std::fabs( coeff * ( pv[ 0 ] + pv[ 1 ] - pv[ 2 ] ) ) );
 143           }
 144           else
 145             r.Fill( S( 0 ) );
 146         }
 147
 148     protected:
 149       InertiaTensorFunction( )
 150         : Superclass( )
 151         {
 152           this->Reset( );
 153         }
 154       virtual ~InertiaTensorFunction( )
 155         {
 156         }
 157
 158     private:
 159       // Purposely not implemented.
 160       InertiaTensorFunction( const Self& );
 161       void operator=( const Self& );
 162
 163     protected:
 164       S m_M;
 165       S m_MPP;
 166       _TInternalVector m_MP;
 167       _TInternalMatrix m_MPPT;
 168     };
 169
 170   } // ecapseman
 171
 172 } // ecapseman
 173
 174 #ifndef ITK_MANUAL_INSTANTIATION
 175 // #include <cpExtensions/Algorithms/InertiaTensorFunction.hxx>
 176 #endif // ITK_MANUAL_INSTANTIATION
 177
 178 #endif // __CPEXTENSIONS__ALGORITHMS__INERTIATENSORFUNCTION__H__
 179
 180 // eof - $RCSfile$