yet another refactoring

[cpPlugins.git] / lib / cpExtensions / Algorithms / InertiaTensorFunction.h

diff --git a/lib/cpExtensions/Algorithms/InertiaTensorFunction.h b/lib/cpExtensions/Algorithms/InertiaTensorFunction.h
new file mode 100644 (file)
index 0000000..ecb341d
--- /dev/null
+++ b/lib/cpExtensions/Algorithms/InertiaTensorFunction.h
@@ -0,0 +1,180 @@
+// -------------------------------------------------------------------------
+// @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
+// -------------------------------------------------------------------------
+
+#ifndef __CPEXTENSIONS__ALGORITHMS__INERTIATENSORFUNCTION__H__
+#define __CPEXTENSIONS__ALGORITHMS__INERTIATENSORFUNCTION__H__
+
+#include <itkObject.h>
+
+#include <itkObject.h>
+#include <itkObjectFactory.h>
+#include <itkMatrix.h>
+#include <itkPoint.h>
+
+
+#include <cmath>
+#include <itkSymmetricEigenAnalysis.h>
+
+
+namespace cpExtensions
+{
+  namespace Algorithms
+  {
+    /**
+     */
+    template< class S, unsigned int D >
+    class InertiaTensorFunction
+      : public itk::Object
+    {
+    public:
+      // Standard itk types
+      typedef InertiaTensorFunction           Self;
+      typedef itk::Object                     Superclass;
+      typedef itk::SmartPointer< Self >       Pointer;
+      typedef itk::SmartPointer< const Self > ConstPointer;
+
+      typedef S                           TScalar;
+      typedef itk::Matrix< S, D, D >      TMatrix;
+      typedef itk::Point< S, D >          TPoint;
+      typedef typename TPoint::VectorType TVector;
+
+    protected:
+      typedef TMatrix _TInternalMatrix;
+      typedef itk::Matrix< S, D, 1 > _TInternalVector;
+
+    public:
+      itkNewMacro( Self );
+      itkTypeMacro( InertiaTensorFunction, itkObject );
+
+    public:
+      inline void Reset( )
+        {
+          this->m_M = S( 0 );
+          this->m_MPP = S( 0 );
+          this->m_MP.Fill( S( 0 ) );
+          this->m_MPPT.Fill( S( 0 ) );
+          this->Modified( );
+        }
+      inline void AddMass( const TPoint& pnt, const S& mass = S( 1 ) )
+        {
+          this->AddMass( pnt.GetVectorFromOrigin( ), mass );
+        }
+      inline void AddMass( const S* data, const S& mass = S( 1 ) )
+        {
+          this->AddMass( TVector( data ), mass );
+        }
+      inline void AddMass( const TVector& vec, const S& mass = S( 1 ) )
+        {
+          this->m_M += mass;
+          this->m_MPP += mass * ( vec * vec );
+
+          _TInternalVector mp;
+          for( unsigned int d = 0; d < D; ++d )
+          {
+            this->m_MP[ d ][ 0 ] += mass * vec[ d ];
+            mp[ d ][ 0 ] = vec[ d ];
+
+          } // rof
+          this->m_MPPT +=
+            _TInternalMatrix( mp.GetVnlMatrix( ) * mp.GetTranspose( ) ) *
+            mass;
+        }
+
+      inline S GetMass( ) const
+        {
+          return( this->m_M );
+        }
+
+      inline TMatrix GetInertia( ) const
+        {
+          TMatrix I;
+          if( S( 0 ) < this->m_M )
+          {
+            I.SetIdentity( );
+            I *= this->m_MPP - ( ( this->m_MP.GetTranspose( ) * this->m_MP.GetVnlMatrix( ) )[ 0 ][ 0 ] / this->m_M );
+            I -= this->m_MPPT;
+            I += TMatrix( this->m_MP.GetVnlMatrix( ) * this->m_MP.GetTranspose( ) ) / this->m_M;
+          }
+          else
+            I.Fill( S( 0 ) );
+          return( I );
+        }
+
+      inline TVector GetCenterOfGravity( ) const
+        {
+          TVector cog;
+          if( S( 0 ) < this->m_M )
+          {
+            for( unsigned int d = 0; d < D; ++d )
+              cog[ d ] = this->m_MP[ d ][ 0 ] / this->m_M;
+          }
+          else
+            cog.Fill( S( 0 ) );
+          return( cog );
+        }
+
+      inline void GetEigenAnalysis( TMatrix& pm, TVector& pv, TVector& r ) const
+        {
+          TMatrix I = this->GetInertia( );
+
+          itk::SymmetricEigenAnalysis< TMatrix, TVector, TMatrix > eigen;
+          eigen.SetDimension( D );
+          eigen.SetOrderEigenMagnitudes( true );
+          eigen.SetOrderEigenValues( 1 );
+          eigen.ComputeEigenValuesAndVectors( I, pv, pm );
+          pm = TMatrix( pm.GetTranspose( ) );
+          S det = vnl_determinant( pm.GetVnlMatrix( ) );
+          for( unsigned int d = 0; d < D; ++d )
+            pm[ d ][ D - 1 ] *= det;
+
+          if( D == 2 )
+          {
+            S coeff = S( 4 ) / this->m_M;
+            r[ 0 ] = std::sqrt( std::fabs( coeff * pv[ 1 ] ) );
+            r[ 1 ] = std::sqrt( std::fabs( coeff * pv[ 0 ] ) );
+          }
+          else if( D == 3 )
+          {
+            S coeff = S( 2.5 ) / this->m_M;
+            r[ 0 ] = std::sqrt( std::fabs( coeff * ( pv[ 1 ] + pv[ 2 ] - pv[ 0 ] ) ) );
+            r[ 1 ] = std::sqrt( std::fabs( coeff * ( pv[ 0 ] + pv[ 2 ] - pv[ 1 ] ) ) );
+            r[ 2 ] = std::sqrt( std::fabs( coeff * ( pv[ 0 ] + pv[ 1 ] - pv[ 2 ] ) ) );
+          }
+          else
+            r.Fill( S( 0 ) );
+        }
+
+    protected:
+      InertiaTensorFunction( )
+        : Superclass( )
+        {
+          this->Reset( );
+        }
+      virtual ~InertiaTensorFunction( )
+        {
+        }
+
+    private:
+      // Purposely not implemented.
+      InertiaTensorFunction( const Self& );
+      void operator=( const Self& );
+
+    protected:
+      S m_M;
+      S m_MPP;
+      _TInternalVector m_MP;
+      _TInternalMatrix m_MPPT;
+    };
+
+  } // ecapseman
+
+} // ecapseman
+
+#ifndef ITK_MANUAL_INSTANTIATION
+// #include <cpExtensions/Algorithms/InertiaTensorFunction.hxx>
+#endif // ITK_MANUAL_INSTANTIATION
+
+#endif // __CPEXTENSIONS__ALGORITHMS__INERTIATENSORFUNCTION__H__
+
+// eof - $RCSfile$