lib/cpm/Algorithms/Base/FindClosestPointInMesh.hxx

   1 #ifndef __CPM__ALGORITHMS__BASE__FINDCLOSESTPOINTINMESH__HXX__
   2 #define __CPM__ALGORITHMS__BASE__FINDCLOSESTPOINTINMESH__HXX__
   3
   4 #include <limits>
   5
   6 // -------------------------------------------------------------------------
   7 template< class M, class S >
   8 void cpm::Algorithms::Base::FindClosestPointInMesh< M, S >::
   9 SetMesh( const M* m )
  10 {
  11   this->m_Mesh = m;
  12   this->Modified( );
  13 }
  14
  15 // -------------------------------------------------------------------------
  16 template< class M, class S >
  17 void cpm::Algorithms::Base::FindClosestPointInMesh< M, S >::
  18 Build( )
  19 {
  20   // Fill in samples
  21   this->m_KDSample = TKDSample::New( );
  22   this->m_KDSample->SetMeasurementVectorSize( TKDVector::Dimension );
  23
  24   TKDVector mv;
  25   for(
  26     unsigned long pId = 0;
  27     pId < this->m_Mesh->GetNumberOfPoints( );
  28     ++pId
  29     )
  30   {
  31     typename M::PointType pnt = this->m_Mesh->GetPoint( pId );
  32     for( unsigned int d = 0; d < TKDVector::Dimension; ++d )
  33       mv[ d ] = S( pnt[ d ] );
  34     mv.PointId = ( typename M::PointIdentifier )( pId );
  35     this->m_KDSample->PushBack( mv );
  36
  37   } // rof
  38
  39   // Compute kd-Tree
  40   this->m_KDGenerator = TKDGenerator::New( );
  41   this->m_KDGenerator->SetSample( this->m_KDSample );
  42   this->m_KDGenerator->SetBucketSize( this->m_BucketSize );
  43   this->m_KDGenerator->Update( );
  44   this->m_KDTree = this->m_KDGenerator->GetOutput( );
  45 }
  46
  47 // -------------------------------------------------------------------------
  48 template< class M, class S >
  49 template< class P >
  50 typename M::PointIdentifier cpm::Algorithms::Base::FindClosestPointInMesh< M, S >::
  51 FindClosestPoint( const P& p ) const
  52 {
  53   TKDVector query;
  54   for( unsigned int d = 0; d < TKDVector::Dimension; ++d )
  55     query[ d ] = S( p[ d ] );
  56
  57   typename TKDTree::InstanceIdentifierVectorType neighs;
  58   std::vector< double > distances;
  59   this->m_KDTree->Search( query, this->m_BucketSize, neighs, distances );
  60   unsigned int minId = 0;
  61   double minD = distances[ 0 ];
  62   for( unsigned int i = 1; i < distances.size( ); ++i )
  63   {
  64     if( distances[ i ] < minD )
  65     {
  66       minId = i;
  67       minD = distances[ i ];
  68
  69     } // fi
  70
  71   } // rof
  72   std::cout << "TODO: " << minD << std::endl;
  73   return( neighs[ minId ] );
  74 }
  75
  76 // -------------------------------------------------------------------------
  77 template< class M, class S >
  78 template< class P, class C >
  79 typename M::PointIdentifier cpm::Algorithms::Base::FindClosestPointInMesh< M, S >::
  80 FindClosestPoint( const P& p, const C& c ) const
  81 {
  82   typename TKDVector::Superclass p1, p2, d;
  83   for( unsigned int i = 0; i < TKDVector::Dimension; ++i )
  84   {
  85     p1[ i ] = S( p[ i ] );
  86     p2[ i ] = S( c[ i ] );
  87     d[ i ] = p1[ i ] - p2[ i ];
  88
  89   } // rof
  90   TKDScalar nd = d.GetNorm( );
  91   typename TKDVector::Superclass n = itk::CrossProduct( p2, p1 );
  92   this->_Dummy( this->m_KDTree->GetRoot( ), d, n, nd );
  93
  94   return( 0 );
  95 }
  96
  97 // -------------------------------------------------------------------------
  98 template< class M, class S >
  99 template< class V, class N >
 100 void cpm::Algorithms::Base::FindClosestPointInMesh< M, S >::
 101 _Dummy( TKDNode* root, const V& d, const V& n, const N& nd ) const
 102 {
 103   typedef
 104     itk::Statistics::KdTreeWeightedCentroidNonterminalNode< TKDSample >
 105     _TNode;
 106   _TNode* node = dynamic_cast< _TNode* >( root );
 107   if( node != NULL )
 108   {
 109     // Get centroids
 110     S left = std::numeric_limits< S >::max( );
 111     S right = left;
 112     if( root->Left( ) != NULL )
 113     {
 114       if( !( root->Left( )->IsTerminal( ) ) )
 115       {
 116         typename _TNode::CentroidType c;
 117         root->Left( )->GetCentroid( c );
 118         left = Self::_LinePointDistance( c, d, n, nd );
 119
 120       } // fi
 121
 122     } // fi
 123     if( root->Right( ) != NULL )
 124     {
 125       if( !( root->Right( )->IsTerminal( ) ) )
 126       {
 127         typename _TNode::CentroidType c;
 128         root->Right( )->GetCentroid( c );
 129         right = Self::_LinePointDistance( c, d, n, nd );
 130
 131       } // fi
 132
 133     } // fi
 134
 135     // Recursive!!!
 136     if( left <= right && root->Left( ) != NULL )
 137       this->_Dummy( root->Left( ), d, n, nd );
 138     else if( left > right && root->Right( ) != NULL )
 139       this->_Dummy( root->Right( ), d, n, nd );
 140     else
 141     {
 142       std::cout << "Something nasty happened" << std::endl;
 143
 144     } // fi
 145
 146   }
 147   else
 148   {
 149     std::cout << "Leaf!!!" << std::endl;
 150
 151   } // fi
 152
 153   /*
 154     _LinePointDistance( const P& p0, const D& d, const N& n, const T& nd )
 155     std::cout << n << " " << d << " " << nd << std::endl;
 156     typedef
 157     itk::Statistics::KdTreeWeightedCentroidNonterminalNode< TKDSample >
 158     _TNode;
 159     _TNode* node = dynamic_cast< _TNode* >( root );
 160     if( node != NULL )
 161     {
 162     // Get centroids
 163     typename _TNode::CentroidType left, right;
 164     if( root->Left( ) != NULL )
 165     root->Left( )->GetCentroid( left );
 166     if( root->Right( ) != NULL )
 167     root->Right( )->GetCentroid( right );
 168
 169     } // fi
 170   */
 171   /*
 172     if( root->Left( ) != NULL )
 173     this->_Dummy( root->Left( ) );
 174     if( root->Right( ) != NULL )
 175     this->_Dummy( root->Right( ) );
 176   */
 177   /*
 178     if( root->IsTerminal( ) )
 179     {
 180     }
 181     else
 182     {
 183     if( root->Left( ) != NULL )
 184     this->_Dummy( root->Left( ) );
 185     if( root->Right( ) != NULL )
 186     this->_Dummy( root->Right( ) );
 187     } // fi
 188   */
 189
 190 }
 191
 192 // -------------------------------------------------------------------------
 193 template< class M, class S >
 194 cpm::Algorithms::Base::FindClosestPointInMesh< M, S >::
 195 FindClosestPointInMesh( )
 196   : Superclass( ),
 197     m_BucketSize( 16 )
 198 {
 199 }
 200
 201 // -------------------------------------------------------------------------
 202 template< class M, class S >
 203 cpm::Algorithms::Base::FindClosestPointInMesh< M, S >::
 204 ~FindClosestPointInMesh( )
 205 {
 206 }
 207
 208 // -------------------------------------------------------------------------
 209 template< class M, class S >
 210 template< class V, class N, class P >
 211 S cpm::Algorithms::Base::FindClosestPointInMesh< M, S >::
 212 _LinePointDistance( const P& p0, const V& d, const V& n, const N& nd )
 213 {
 214   V v0;
 215   for( unsigned int i = 0; i < Self::Dimension; ++i )
 216     v0[ i ] = N( p0[ i ] );
 217   return( S( ( itk::CrossProduct( v0, d ) - n ).GetNorm( ) / nd ) );
 218 }
 219
 220 #endif // __CPM__ALGORITHMS__BASE__FINDCLOSESTPOINTINMESH__HXX__
 221
 222 // eof - $RCSfile$