lib/cpPlugins/Extensions/Algorithms/InertiaTensorFunction.h

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