1 // -------------------------------------------------------------------------
2 // @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
3 // -------------------------------------------------------------------------
5 #ifndef __CPPLUGINS__EXTENSIONS__ALGORITHMS__INERTIATENSORFUNCTION__H__
6 #define __CPPLUGINS__EXTENSIONS__ALGORITHMS__INERTIATENSORFUNCTION__H__
10 #include <itkObject.h>
11 #include <itkObjectFactory.h>
12 #include <itkMatrix.h>
17 #include <itkSymmetricEigenAnalysis.h>
28 template< class S, unsigned int D >
29 class InertiaTensorFunction
34 typedef InertiaTensorFunction Self;
35 typedef itk::Object Superclass;
36 typedef itk::SmartPointer< Self > Pointer;
37 typedef itk::SmartPointer< const Self > ConstPointer;
40 typedef itk::Matrix< S, D, D > TMatrix;
41 typedef itk::Point< S, D > TPoint;
42 typedef typename TPoint::VectorType TVector;
45 typedef TMatrix _TInternalMatrix;
46 typedef itk::Matrix< S, D, 1 > _TInternalVector;
50 itkTypeMacro( InertiaTensorFunction, itkObject );
57 this->m_MP.Fill( S( 0 ) );
58 this->m_MPPT.Fill( S( 0 ) );
61 inline void AddMass( const TPoint& pnt, const S& mass = S( 1 ) )
63 this->AddMass( pnt.GetVectorFromOrigin( ), mass );
65 inline void AddMass( const S* data, const S& mass = S( 1 ) )
67 this->AddMass( TVector( data ), mass );
69 inline void AddMass( const TVector& vec, const S& mass = S( 1 ) )
72 this->m_MPP += mass * ( vec * vec );
75 for( unsigned int d = 0; d < D; ++d )
77 this->m_MP[ d ][ 0 ] += mass * vec[ d ];
78 mp[ d ][ 0 ] = vec[ d ];
82 _TInternalMatrix( mp.GetVnlMatrix( ) * mp.GetTranspose( ) ) *
86 inline S GetMass( ) const
91 inline TMatrix GetInertia( ) const
94 if( S( 0 ) < this->m_M )
97 I *= this->m_MPP - ( ( this->m_MP.GetTranspose( ) * this->m_MP.GetVnlMatrix( ) )[ 0 ][ 0 ] / this->m_M );
99 I += TMatrix( this->m_MP.GetVnlMatrix( ) * this->m_MP.GetTranspose( ) ) / this->m_M;
106 inline TVector GetCenterOfGravity( ) const
109 if( S( 0 ) < this->m_M )
111 for( unsigned int d = 0; d < D; ++d )
112 cog[ d ] = this->m_MP[ d ][ 0 ] / this->m_M;
119 inline void GetEigenAnalysis( TMatrix& pm, TVector& pv, TVector& r ) const
121 TMatrix I = this->GetInertia( );
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;
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 ] ) );
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 ] ) ) );
151 InertiaTensorFunction( )
156 virtual ~InertiaTensorFunction( )
161 // Purposely not implemented.
162 InertiaTensorFunction( const Self& );
163 void operator=( const Self& );
168 _TInternalVector m_MP;
169 _TInternalMatrix m_MPPT;
178 // #include <cpPlugins/Extensions/Algorithms/InertiaTensorFunction.hxx>
180 #endif // __CPPLUGINS__EXTENSIONS__ALGORITHMS__INERTIATENSORFUNCTION__H__