OpenWalnut  1.4.0
WThreadedTrackingFunction.cpp
00001 //---------------------------------------------------------------------------
00002 //
00003 // Project: OpenWalnut ( http://www.openwalnut.org )
00004 //
00005 // Copyright 2009 OpenWalnut Community, BSV@Uni-Leipzig and CNCF@MPI-CBS
00006 // For more information see http://www.openwalnut.org/copying
00007 //
00008 // This file is part of OpenWalnut.
00009 //
00010 // OpenWalnut is free software: you can redistribute it and/or modify
00011 // it under the terms of the GNU Lesser General Public License as published by
00012 // the Free Software Foundation, either version 3 of the License, or
00013 // (at your option) any later version.
00014 //
00015 // OpenWalnut is distributed in the hope that it will be useful,
00016 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00017 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018 // GNU Lesser General Public License for more details.
00019 //
00020 // You should have received a copy of the GNU Lesser General Public License
00021 // along with OpenWalnut. If not, see <http://www.gnu.org/licenses/>.
00022 //
00023 //---------------------------------------------------------------------------
00024 
00025 #include <limits>
00026 #include <string>
00027 #include <vector>
00028 
00029 #include "../common/WLimits.h"
00030 #include "WThreadedTrackingFunction.h"
00031 
00032 namespace wtracking
00033 {
00034     bool WTrackingUtility::followToNextVoxel( DataSetPtr dataset, JobType& job, DirFunc const& dirFunc )
00035     {
00036         // TODO(reichenbach): Give those WAsserts correct strings incase they occur
00037         WAssert( dataset->getGrid(), "" );
00038         Grid3DPtr g = boost::dynamic_pointer_cast< WGridRegular3D >( dataset->getGrid() );
00039         WAssert( g, "" );
00040         WAssert( g->encloses( job.first ), "" );
00041 
00042         // find matching direction
00043         WVector3d dir = dirFunc( dataset, job );
00044 
00045         if( fabs( length( dir ) - 1.0 ) > TRACKING_EPS )
00046         {
00047             return false;
00048         }
00049 
00050         // find t such that job.first() + t * dir is a point on the boundary of the current voxel
00051         double t = getDistanceToBoundary( g, job.first, dir );
00052         WAssert( !wlimits::isInf( t ) && !wlimits::isNaN( t ), "Warning in WTrackingUtility::followToNextVoxel NaN's or INF's occured" );
00053         WAssert( t > 0.0, "" );
00054         WAssert( onBoundary( g, job.first + dir * t ), "" );
00055 
00056         if( !g->encloses( job.first + dir * t ) )
00057         {
00058             return false;
00059         }
00060 
00061         // increase t, so that job.first() + t * dir does not lie on any boundary anymore
00062         // ( so we can find the corresponding voxel without ambiguities )
00063         // ( this also means the resulting new position will never be in the same
00064         //   voxel as the starting position )
00065         int i = 0;
00066         while( onBoundary( g, job.first + dir * t ) && i < 50 )
00067         {
00068             t += TRACKING_EPS;
00069             if( !g->encloses( job.first + dir * t ) )
00070             {
00071                 return false;
00072             }
00073             ++i;
00074         }
00075         if( i == 50 )
00076         {
00077             return false;
00078         }
00079 
00080         // set the new position
00081         job.first += t * dir;
00082         job.second = dir;
00083         return true;
00084     }
00085 
00086     bool WTrackingUtility::onBoundary( Grid3DPtr grid, WVector3d const& pos )
00087     {
00088         WVector3d v = pos - grid->getOrigin();
00089         double p;
00090         double z;
00091         WVector3d d = grid->getDirectionX();
00092         d = normalize( d );
00093         double c = dot( d, v ) / grid->getOffsetX() + 0.5;
00094         p = c >= 0.0 ? modf( c, &z ) : 1.0 + modf( c, &z );
00095         if( fabs( p ) < TRACKING_EPS || fabs( p - 1 ) < TRACKING_EPS )
00096         {
00097             return true;
00098         }
00099         d = grid->getDirectionY();
00100         d = normalize( d );
00101         c = dot( d,  v ) / grid->getOffsetY() + 0.5;
00102         p = c >= 0.0 ? modf( c, &z ) : 1.0 + modf( c, &z );
00103         if( fabs( p ) < TRACKING_EPS || fabs( p - 1 ) < TRACKING_EPS )
00104         {
00105             return true;
00106         }
00107         d = grid->getDirectionZ();
00108         d = normalize( d );
00109         c = dot( d, v ) / grid->getOffsetZ() + 0.5;
00110         p = c >= 0.0 ? modf( c, &z ) : 1.0 + modf( c, &z );
00111         if( fabs( p ) < TRACKING_EPS || fabs( p - 1 ) < TRACKING_EPS )
00112         {
00113             return true;
00114         }
00115         return false;
00116     }
00117 
00118     double WTrackingUtility::getDistanceToBoundary( Grid3DPtr grid, WVector3d const& pos, WVector3d const& dir )
00119     {
00120         // the input pos is at least TRACKING_EPS away from the boundary
00121         // so distance calculations don't get screwed
00122         WAssert( !onBoundary( grid, pos ), "" );
00123 
00124         double dist = std::numeric_limits< double >::infinity();
00125 
00126         WVector3d v = pos - grid->getOrigin();
00127         WVector3d p;
00128         double z;
00129         WVector3d o( grid->getOffsetX(), grid->getOffsetY(), grid->getOffsetZ() );
00130         WVector3d s[ 3 ] = { grid->getDirectionX() / o[ 0 ], grid->getDirectionY() / o[ 1 ], grid->getDirectionZ() / o[ 2 ] };
00131         double c = dot( s[ 0 ], v ) / o[ 0 ] + 0.5;
00132         p[ 0 ] = c >= 0.0 ? modf( c, &z ) : 1.0 + modf( c, &z );
00133         c = dot( s[ 1 ], v ) / o[ 1 ] + 0.5;
00134         p[ 1 ] = c >= 0.0 ? modf( c, &z ) : 1.0 + modf( c, &z );
00135         c = dot( s[ 2 ], v ) / o[ 2 ] + 0.5;
00136         p[ 2 ] = c >= 0.0 ? modf( c, &z ) : 1.0 + modf( c, &z );
00137         WVector3d d( dot( dir, s[ 0 ] ), dot( dir, s[ 1 ] ), dot( dir, s[ 2 ] ) );
00138 
00139         double t;
00140         for( int i = 0; i < 3; ++i )
00141         {
00142             WAssert( -TRACKING_EPS < p[ i ] && p[ i ] < o[ i ] + TRACKING_EPS, "" );
00143             for( double j = 0.0; j <= 1.0; j += 1.0 )
00144             {
00145                 if( fabs( d[ i ] ) > TRACKING_EPS )
00146                 {
00147                     t = ( j - p[ i ] ) * o[ i ] / d[ i ];
00148                     if( t > 0.0 && t < dist )
00149                     {
00150                         dist = t;
00151                     }
00152                 }
00153             }
00154         }
00155         // distance absolute must be at least TRACKING_EPS!
00156         WAssert( dist >= TRACKING_EPS, "" );
00157         return dist;
00158     }
00159 
00160     //////////////////////////////////////////////////////////////////////////////////////////
00161 
00162     WThreadedTrackingFunction::WThreadedTrackingFunction( DataSetPtr dataset, DirFunc dirFunc,
00163             NextPositionFunc nextFunc,
00164             FiberVisitorFunc fiberVst, PointVisitorFunc pointVst,
00165             std::size_t seedPositions, std::size_t seedsPerPos,
00166             std::vector< int > v0, std::vector< int > v1 )
00167         : Base( dataset ),
00168         m_grid( boost::dynamic_pointer_cast< GridType >( dataset->getGrid() ) ),
00169         m_directionFunc( dirFunc ),
00170         m_nextPosFunc( nextFunc ),
00171         m_fiberVisitor( fiberVst ),
00172         m_pointVisitor( pointVst ),
00173         m_maxPoints(),
00174         m_currentIndex()
00175     {
00176         // dataset != 0 is tested by the base constructor
00177         if( !m_grid )
00178         {
00179             throw WException( std::string( "Cannot find WGridRegular3D. Are you sure the dataset has the correct grid type?" ) );
00180         }
00181 
00182         m_maxPoints = static_cast< std::size_t >( 5 * pow( static_cast< double >( m_grid->size() ), 1.0 / 3.0 ) );
00183 
00184         m_currentIndex.getWriteTicket()->get() = IndexType( m_grid, v0, v1, seedPositions, seedsPerPos );
00185     }
00186 
00187     WThreadedTrackingFunction::~WThreadedTrackingFunction()
00188     {
00189     }
00190 
00191     bool WThreadedTrackingFunction::getJob( JobType& job )  // NOLINT
00192     {
00193         WSharedObject< IndexType >::WriteTicket t = m_currentIndex.getWriteTicket();
00194         if( t->get().done() )
00195         {
00196             return false;
00197         }
00198         else
00199         {
00200             job = t->get().job();
00201             ++t->get();
00202             return true;
00203         }
00204     }
00205 
00206     void WThreadedTrackingFunction::compute( DataSetPtr input, JobType const& job )
00207     {
00208         WVector3d e = m_directionFunc( input, job );
00209         JobType j = job;
00210         j.second = e;
00211 
00212         std::vector< WVector3d > fiber;
00213         if( fabs( length( e ) - 1.0 ) > TRACKING_EPS )
00214         {
00215             if( m_fiberVisitor )
00216             {
00217                 m_fiberVisitor( fiber );
00218             }
00219             return;
00220         }
00221 
00222         fiber.push_back( j.first );
00223         if( m_pointVisitor )
00224         {
00225             m_pointVisitor( j.first );
00226         }
00227 
00228         // forward integration
00229         for( std::size_t k = 0; k < m_maxPoints; ++k )
00230         {
00231             if( fabs( length( j.second ) - 1.0 ) > TRACKING_EPS )
00232             {
00233                 break;
00234             }
00235             if( !m_nextPosFunc( input, j, m_directionFunc ) )
00236             {
00237                 break;
00238             }
00239             fiber.push_back( j.first );
00240             if( m_pointVisitor )
00241             {
00242                 m_pointVisitor( j.first );
00243             }
00244         }
00245 
00246         // backward integration
00247         std::reverse( fiber.begin(), fiber.end() );
00248         j = job;
00249         j.second = e * -1.0;
00250         for( std::size_t k = 0; k < m_maxPoints; ++k )
00251         {
00252             if( fabs( length( j.second ) - 1.0 ) > TRACKING_EPS )
00253             {
00254                 break;
00255             }
00256             if( !m_nextPosFunc( input, j, m_directionFunc ) )
00257             {
00258                 break;
00259             }
00260             fiber.push_back( j.first );
00261             if( m_pointVisitor )
00262             {
00263                 m_pointVisitor( j.first );
00264             }
00265         }
00266 
00267         // output result
00268         if( m_fiberVisitor )
00269         {
00270             m_fiberVisitor( fiber );
00271         }
00272     }
00273 
00274     WThreadedTrackingFunction::IndexType::IndexType()
00275         :   m_grid(),
00276         m_done( true )
00277     {
00278     }
00279 
00280     WThreadedTrackingFunction::IndexType::IndexType( GridPtr grid, std::vector< int > const& v0,
00281             std::vector< int > const& v1, std::size_t seedPositions,
00282             std::size_t seedsPerPosition )
00283         : m_grid( grid ),
00284         m_done( false )
00285     {
00286         if( v0.size() != 3 )
00287         {
00288             m_min[ 0 ] = m_min[ 1 ] = m_min[ 2 ] = seedPositions;
00289         }
00290         else
00291         {
00292             WAssert( v0[ 0 ] >= 1 && static_cast< std::size_t >( v0[ 0 ] ) < m_grid->getNbCoordsX() - 1, "" );
00293             WAssert( v0[ 1 ] >= 1 && static_cast< std::size_t >( v0[ 1 ] ) < m_grid->getNbCoordsY() - 1, "" );
00294             WAssert( v0[ 2 ] >= 1 && static_cast< std::size_t >( v0[ 2 ] ) < m_grid->getNbCoordsZ() - 1, "" );
00295             for( int i = 0; i < 3; ++i )
00296             {
00297                 m_min[ i ] = v0[ i ] * seedPositions;
00298             }
00299         }
00300         if( v1.size() != 3 )
00301         {
00302             m_max[ 0 ] = ( m_grid->getNbCoordsX() - 1 ) * seedPositions;
00303             m_max[ 1 ] = ( m_grid->getNbCoordsY() - 1 ) * seedPositions;
00304             m_max[ 2 ] = ( m_grid->getNbCoordsZ() - 1 ) * seedPositions;
00305         }
00306         else
00307         {
00308             WAssert( v1[ 0 ] > v0[ 0 ] && static_cast< std::size_t >( v1[ 0 ] ) < grid->getNbCoordsX(), "" );
00309             WAssert( v1[ 1 ] > v0[ 1 ] && static_cast< std::size_t >( v1[ 1 ] ) < grid->getNbCoordsY(), "" );
00310             WAssert( v1[ 2 ] > v0[ 2 ] && static_cast< std::size_t >( v1[ 2 ] ) < grid->getNbCoordsZ(), "" );
00311             for( int i = 0; i < 3; ++i )
00312             {
00313                 m_max[ i ] = v1[ i ] * seedPositions;
00314             }
00315         }
00316         m_offset = 1.0 / seedPositions;
00317         m_min[ 3 ] = 0;
00318         m_max[ 3 ] = seedsPerPosition;
00319         m_pos[ 0 ] = m_min[ 0 ];
00320         m_pos[ 1 ] = m_min[ 1 ];
00321         m_pos[ 2 ] = m_min[ 2 ];
00322         m_pos[ 3 ] = 0;
00323     }
00324 
00325     WThreadedTrackingFunction::IndexType& WThreadedTrackingFunction::IndexType::operator++ ()
00326     {
00327         if( !m_done )
00328         {
00329             for( int i = 3; i > -1; --i )
00330             {
00331                 ++m_pos[ i ];
00332                 if( m_pos[ i ] == m_max[ i ] )
00333                 {
00334                     m_pos[ i ] = m_min[ i ];
00335                     m_done = i == 0;
00336                 }
00337                 else
00338                 {
00339                     break;
00340                 };
00341             }
00342         }
00343         return *this;
00344     }
00345 
00346     bool WThreadedTrackingFunction::IndexType::done()
00347     {
00348         return m_done;
00349     }
00350 
00351     WThreadedTrackingFunction::JobType WThreadedTrackingFunction::IndexType::job()
00352     {
00353         JobType job;
00354         job.second = WVector3d( 0.0, 0.0, 0.0 );
00355         job.first = m_grid->getOrigin() + m_grid->getDirectionX() * ( m_offset * ( 0.5 + m_pos[ 0 ] ) - 0.5 )
00356             + m_grid->getDirectionY() * ( m_offset * ( 0.5 + m_pos[ 1 ] ) - 0.5 )
00357             + m_grid->getDirectionZ() * ( m_offset * ( 0.5 + m_pos[ 2 ] ) - 0.5 );
00358         return job;
00359     }
00360 
00361 }  // namespace wtracking