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 // #include <cpExtensions/Algorithms/InertiaTensorFunction.hxx>
 175
 176 #endif // __CPEXTENSIONS__ALGORITHMS__INERTIATENSORFUNCTION__H__
 177
 178 // eof - $RCSfile$