1 // -------------------------------------------------------------------------
2 // @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
3 // -------------------------------------------------------------------------
5 #ifndef __CPEXTENSIONS__ALGORITHMS__INERTIATENSORFUNCTION__H__
6 #define __CPEXTENSIONS__ALGORITHMS__INERTIATENSORFUNCTION__H__
10 #include <itkObject.h>
11 #include <itkObjectFactory.h>
12 #include <itkMatrix.h>
17 #include <itkSymmetricEigenAnalysis.h>
20 namespace cpExtensions
26 template< class S, unsigned int D >
27 class InertiaTensorFunction
32 typedef InertiaTensorFunction Self;
33 typedef itk::Object Superclass;
34 typedef itk::SmartPointer< Self > Pointer;
35 typedef itk::SmartPointer< const Self > ConstPointer;
38 typedef itk::Matrix< S, D, D > TMatrix;
39 typedef itk::Point< S, D > TPoint;
40 typedef typename TPoint::VectorType TVector;
43 typedef TMatrix _TInternalMatrix;
44 typedef itk::Matrix< S, D, 1 > _TInternalVector;
48 itkTypeMacro( InertiaTensorFunction, itkObject );
55 this->m_MP.Fill( S( 0 ) );
56 this->m_MPPT.Fill( S( 0 ) );
59 inline void AddMass( const TPoint& pnt, const S& mass = S( 1 ) )
61 this->AddMass( pnt.GetVectorFromOrigin( ), mass );
63 inline void AddMass( const S* data, const S& mass = S( 1 ) )
65 this->AddMass( TVector( data ), mass );
67 inline void AddMass( const TVector& vec, const S& mass = S( 1 ) )
70 this->m_MPP += mass * ( vec * vec );
73 for( unsigned int d = 0; d < D; ++d )
75 this->m_MP[ d ][ 0 ] += mass * vec[ d ];
76 mp[ d ][ 0 ] = vec[ d ];
80 _TInternalMatrix( mp.GetVnlMatrix( ) * mp.GetTranspose( ) ) *
84 inline S GetMass( ) const
89 inline TMatrix GetInertia( ) const
92 if( S( 0 ) < this->m_M )
95 I *= this->m_MPP - ( ( this->m_MP.GetTranspose( ) * this->m_MP.GetVnlMatrix( ) )[ 0 ][ 0 ] / this->m_M );
97 I += TMatrix( this->m_MP.GetVnlMatrix( ) * this->m_MP.GetTranspose( ) ) / this->m_M;
104 inline TVector GetCenterOfGravity( ) const
107 if( S( 0 ) < this->m_M )
109 for( unsigned int d = 0; d < D; ++d )
110 cog[ d ] = this->m_MP[ d ][ 0 ] / this->m_M;
117 inline void GetEigenAnalysis( TMatrix& pm, TVector& pv, TVector& r ) const
119 TMatrix I = this->GetInertia( );
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;
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 ] ) );
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 ] ) ) );
149 InertiaTensorFunction( )
154 virtual ~InertiaTensorFunction( )
159 // Purposely not implemented.
160 InertiaTensorFunction( const Self& );
161 void operator=( const Self& );
166 _TInternalVector m_MP;
167 _TInternalMatrix m_MPPT;
174 #ifndef ITK_MANUAL_INSTANTIATION
175 // #include <cpExtensions/Algorithms/InertiaTensorFunction.hxx>
176 #endif // ITK_MANUAL_INSTANTIATION
178 #endif // __CPEXTENSIONS__ALGORITHMS__INERTIATENSORFUNCTION__H__