OpenWalnut  1.4.0
WTensorFunctions.h
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 #ifndef WTENSORFUNCTIONS_H
00026 #define WTENSORFUNCTIONS_H
00027 
00028 #include <algorithm>
00029 #include <cmath>
00030 #include <complex>
00031 #include <iostream>
00032 #include <utility>
00033 #include <vector>
00034 
00035 #include <boost/array.hpp>
00036 
00037 #include <Eigen/Dense>
00038 
00039 #include "../WAssert.h"
00040 #include "../WLimits.h"
00041 #include "WCompileTimeFunctions.h"
00042 #include "WTensor.h"
00043 #include "WTensorSym.h"
00044 #include "WMatrix.h"
00045 
00046 /**
00047  * An eigensystem has all eigenvalues as well as its corresponding eigenvectors. A RealEigenSystem is an EigenSystem where all
00048  * eigenvalues are real and not complex.
00049  */
00050 typedef boost::array< std::pair< double, WVector3d >, 3 > RealEigenSystem;
00051 
00052 /**
00053  * An eigensystem has all eigenvalues as well its corresponding eigenvectors.
00054  */
00055 typedef boost::array< std::pair< std::complex< double >, WVector3d >, 3 > EigenSystem;
00056 
00057 std::ostream& operator<<( std::ostream& os, const RealEigenSystem& sys );
00058 
00059 /**
00060  * Helper function to sort eigen values on their size. As each eigenvalues has an eigenvector they must be twiddled the same.
00061  *
00062  * \param es Eigensystem consisting of eigenvalues and eigenvectors.
00063  */
00064 void sortRealEigenSystem( RealEigenSystem* es )
00065 {
00066     if( ( *es )[0].first > ( *es )[2].first )
00067     {
00068         std::swap( ( *es )[0], ( *es )[2] );
00069     }
00070     if( ( *es )[0].first > ( *es )[1].first )
00071     {
00072         std::swap( ( *es )[0], ( *es )[1] );
00073     }
00074     if( ( *es )[1].first > ( *es )[2].first )
00075     {
00076         std::swap( ( *es )[1], ( *es )[2] );
00077     }
00078 }
00079 
00080 /**
00081  * Compute all eigenvalues as well as the corresponding eigenvectors of a
00082  * symmetric real Matrix.
00083  *
00084  * \note Data_T must be castable to double.
00085  *
00086  * \param[in] mat A real symmetric matrix.
00087  * \param[out] RealEigenSystem A pointer to an RealEigenSystem.
00088  */
00089 template< typename Data_T >
00090 void jacobiEigenvector3D( WTensorSym< 2, 3, Data_T > const& mat, RealEigenSystem* es )
00091 {
00092     RealEigenSystem& result = *es; // alias for the result
00093     WTensorSym< 2, 3, Data_T > in( mat );
00094     WTensor< 2, 3, Data_T > ev;
00095     ev( 0, 0 ) = ev( 1, 1 ) = ev( 2, 2 ) = 1.0;
00096 
00097     int iter = 50;
00098     Data_T evp[ 3 ];
00099     Data_T evq[ 3 ];
00100 
00101     while( iter >= 0 )
00102     {
00103         int p = 1;
00104         int q = 0;
00105 
00106         for( int i = 0; i < 2; ++i )
00107         {
00108             if( fabs( in( 2, i ) ) > fabs( in( p, q ) ) )
00109             {
00110                 p = 2;
00111                 q = i;
00112             }
00113         }
00114 
00115         // Note: If all non diagonal elements sum up to nearly zero, we may quit already!
00116         // Thereby the chosen threshold 1.0e-50 was taken arbitrarily and is just a guess.
00117         if( std::abs( in( 0, 1 ) ) + std::abs( in( 0, 2 ) ) + std::abs( in( 1, 2 ) ) < 1.0e-50 )
00118         {
00119             for( int i = 0; i < 3; ++i )
00120             {
00121                 result[i].first = in( i, i );
00122                 for( int j = 0; j < 3; ++j )
00123                 {
00124                     result[i].second[j] = static_cast< double >( ev( j, i ) );
00125                 }
00126             }
00127             sortRealEigenSystem( es );
00128             return;
00129         }
00130 
00131         Data_T r = in( q, q ) - in( p, p );
00132         Data_T o = r / ( 2.0 * in( p, q ) );
00133 
00134         Data_T t;
00135         Data_T signofo = ( o < 0.0 ? -1.0 : 1.0 );
00136         if( o * o > wlimits::MAX_DOUBLE )
00137         {
00138             t = signofo / ( 2.0 * fabs( o ) );
00139         }
00140         else
00141         {
00142             t = signofo / ( fabs( o ) + sqrt( o * o + 1.0 ) );
00143         }
00144 
00145         Data_T c;
00146 
00147         if( t * t < wlimits::DBL_EPS )
00148         {
00149             c = 1.0;
00150         }
00151         else
00152         {
00153             c = 1.0 / sqrt( t * t + 1.0 );
00154         }
00155 
00156         Data_T s = c * t;
00157 
00158         int k = 0;
00159         while( k == q || k == p )
00160         {
00161             ++k;
00162         }
00163 
00164         Data_T u = ( 1.0 - c ) / s;
00165 
00166         Data_T x = in( k, p );
00167         Data_T y = in( k, q );
00168         in( p, k ) = in( k, p ) = x - s * ( y + u * x );
00169         in( q, k ) = in( k, q ) = y + s * ( x - u * y );
00170         x = in( p, p );
00171         y = in( q, q );
00172         in( p, p ) = x - t * in( p, q );
00173         in( q, q ) = y + t * in( p, q );
00174         in( q, p ) = in( p, q ) = 0.0;
00175 
00176         for( int l = 0; l < 3; ++l )
00177         {
00178             evp[ l ] = ev( l, p );
00179             evq[ l ] = ev( l, q );
00180         }
00181         for( int l = 0; l < 3; ++l )
00182         {
00183             ev( l, p ) = c * evp[ l ] - s * evq[ l ];
00184             ev( l, q ) = s * evp[ l ] + c * evq[ l ];
00185         }
00186 
00187         --iter;
00188     }
00189     WAssert( iter >= 0, "Jacobi eigenvector iteration did not converge." );
00190 }
00191 
00192 /**
00193  * Compute all eigenvalues as well as the corresponding eigenvectors of a
00194  * symmetric real Matrix.
00195  *
00196  * \note Data_T must be castable to double.
00197  *
00198  * \param[in] mat A real symmetric matrix.
00199  * \param[out] RealEigenSystem A pointer to an RealEigenSystem.
00200  */
00201 template< typename Data_T >
00202 void eigenEigenvector3D( WTensorSym< 2, 3, Data_T > const& mat, RealEigenSystem* es )
00203 {
00204     RealEigenSystem& result = *es; // alias for the result
00205     typedef Eigen::Matrix< Data_T, 3, 3 > MatrixType;
00206 
00207     MatrixType m;
00208     m << mat( 0, 0 ), mat( 0, 1 ), mat( 0, 2 ),
00209          mat( 1, 0 ), mat( 1, 1 ), mat( 1, 2 ),
00210          mat( 2, 0 ), mat( 2, 1 ), mat( 2, 2 );
00211 
00212     Eigen::SelfAdjointEigenSolver< MatrixType > solv( m );
00213 
00214     for( std::size_t k = 0; k < 3; ++k )
00215     {
00216         result[ k ].first = solv.eigenvalues()[ k ];
00217         for( std::size_t j = 0; j < 3; ++j )
00218         {
00219             result[ k ].second[ j ] = solv.eigenvectors()( j, k );
00220         }
00221     }
00222 }
00223 
00224 /**
00225  * Calculate eigenvectors via the characteristic polynomial. This is essentially the same
00226  * function as in the GPU glyph shaders. This is for 3 dimensions only.
00227  *
00228  * \param m The symmetric matrix to calculate the eigenvalues from.
00229  * \return A std::vector of 3 eigenvalues in descending order (of their magnitude).
00230  */
00231 std::vector< double > getEigenvaluesCardano( WTensorSym< 2, 3 > const& m );
00232 
00233 /**
00234  * Multiply tensors of order 2. This is essentially a matrix-matrix multiplication.
00235  *
00236  * Tensors must have the same data type and dimension, however both symmetric and asymmetric
00237  * tensors (in any combination) are allowed as operands. The returned tensor is always an asymmetric tensor.
00238  *
00239  * \param one The first operand, a WTensor or WTensorSym of order 2.
00240  * \param other The second operand, a WTensor or WTensorSym of order 2.
00241  *
00242  * \return A WTensor that is the product of the operands.
00243  */
00244 template< template< std::size_t, std::size_t, typename > class TensorType1, // NOLINT brainlint can't find TensorType1
00245           template< std::size_t, std::size_t, typename > class TensorType2, // NOLINT
00246           std::size_t dim,
00247           typename Data_T >
00248 WTensor< 2, dim, Data_T > operator * ( TensorType1< 2, dim, Data_T > const& one,
00249                                        TensorType2< 2, dim, Data_T > const& other )
00250 {
00251     WTensor< 2, dim, Data_T > res;
00252 
00253     // calc res_ij = one_ik * other_kj
00254     for( std::size_t i = 0; i < dim; ++i )
00255     {
00256         for( std::size_t j = 0; j < dim; ++j )
00257         {
00258             // components are initialized to zero, so there is no need to zero them here
00259             for( std::size_t k = 0; k < dim; ++k )
00260             {
00261                 res( i, j ) += one( i, k ) * other( k, j );
00262             }
00263         }
00264     }
00265 
00266     return res;
00267 }
00268 
00269 /**
00270  * Evaluate a spherical function represented by a symmetric 4th-order tensor for a given gradient.
00271  *
00272  * \tparam Data_T The integral type used to store the tensor elements.
00273  *
00274  * \param tens The tensor representing the spherical function.
00275  * \param gradient The normalized vector that represents the gradient direction.
00276  *
00277  * \note If the gradient is not normalized, the result is undefined.
00278  */
00279 template< typename Data_T >
00280 double evaluateSphericalFunction( WTensorSym< 4, 3, Data_T > const& tens, WVector3d const& gradient )
00281 {
00282     // use symmetry to reduce computation overhead
00283     // temporaries for some of the gradient element multiplications could further reduce
00284     // computation time
00285     return gradient[ 0 ] * gradient[ 0 ] * gradient[ 0 ] * gradient[ 0 ] * tens( 0, 0, 0, 0 )
00286          + gradient[ 1 ] * gradient[ 1 ] * gradient[ 1 ] * gradient[ 1 ] * tens( 1, 1, 1, 1 )
00287          + gradient[ 2 ] * gradient[ 2 ] * gradient[ 2 ] * gradient[ 2 ] * tens( 2, 2, 2, 2 )
00288          + static_cast< Data_T >( 4 ) *
00289          ( gradient[ 0 ] * gradient[ 0 ] * gradient[ 0 ] * gradient[ 1 ] * tens( 0, 0, 0, 1 )
00290          + gradient[ 0 ] * gradient[ 0 ] * gradient[ 0 ] * gradient[ 2 ] * tens( 0, 0, 0, 2 )
00291          + gradient[ 1 ] * gradient[ 1 ] * gradient[ 1 ] * gradient[ 0 ] * tens( 1, 1, 1, 0 )
00292          + gradient[ 2 ] * gradient[ 2 ] * gradient[ 2 ] * gradient[ 0 ] * tens( 2, 2, 2, 0 )
00293          + gradient[ 1 ] * gradient[ 1 ] * gradient[ 1 ] * gradient[ 2 ] * tens( 1, 1, 1, 2 )
00294          + gradient[ 2 ] * gradient[ 2 ] * gradient[ 2 ] * gradient[ 1 ] * tens( 2, 2, 2, 1 ) )
00295          + static_cast< Data_T >( 12 ) *
00296          ( gradient[ 2 ] * gradient[ 1 ] * gradient[ 0 ] * gradient[ 0 ] * tens( 2, 1, 0, 0 )
00297          + gradient[ 0 ] * gradient[ 2 ] * gradient[ 1 ] * gradient[ 1 ] * tens( 0, 2, 1, 1 )
00298          + gradient[ 0 ] * gradient[ 1 ] * gradient[ 2 ] * gradient[ 2 ] * tens( 0, 1, 2, 2 ) )
00299          + static_cast< Data_T >( 6 ) *
00300          ( gradient[ 0 ] * gradient[ 0 ] * gradient[ 1 ] * gradient[ 1 ] * tens( 0, 0, 1, 1 )
00301          + gradient[ 0 ] * gradient[ 0 ] * gradient[ 2 ] * gradient[ 2 ] * tens( 0, 0, 2, 2 )
00302          + gradient[ 1 ] * gradient[ 1 ] * gradient[ 2 ] * gradient[ 2 ] * tens( 1, 1, 2, 2 ) );
00303 }
00304 
00305 /**
00306  * Evaluate a spherical function represented by a symmetric 2nd-order tensor for a given gradient.
00307  *
00308  * \tparam Data_T The integral type used to store the tensor elements.
00309  *
00310  * \param tens The tensor representing the spherical function.
00311  * \param gradient The normalized vector that represents the gradient direction.
00312  *
00313  * \note If the gradient is not normalized, the result is undefined.
00314  */
00315 template< typename Data_T >
00316 double evaluateSphericalFunction( WTensorSym< 2, 3, Data_T > const& tens, WVector3d const& gradient )
00317 {
00318     return gradient[ 0 ] * gradient[ 0 ] * tens( 0, 0 )
00319          + gradient[ 1 ] * gradient[ 1 ] * tens( 1, 1 )
00320          + gradient[ 2 ] * gradient[ 2 ] * tens( 2, 2 )
00321          + static_cast< Data_T >( 2 ) *
00322          ( gradient[ 0 ] * gradient[ 1 ] * tens( 0, 1 )
00323          + gradient[ 0 ] * gradient[ 2 ] * tens( 0, 2 )
00324          + gradient[ 1 ] * gradient[ 2 ] * tens( 1, 2 ) );
00325 }
00326 
00327 /**
00328  * Calculate the logarithm of the given real symmetric tensor.
00329  *
00330  * \param tens The tensor.
00331  * \return The logarithm of the tensor.
00332  */
00333 template< typename T >
00334 WTensorSym< 2, 3, T > tensorLog( WTensorSym< 2, 3, T > const& tens )
00335 {
00336     WTensorSym< 2, 3, T > res;
00337 
00338     // calculate eigenvalues and eigenvectors
00339     RealEigenSystem sys;
00340     eigenEigenvector3D( tens, &sys );
00341 
00342     // first, we check for negative or zero eigenvalues
00343     if( sys[ 0 ].first <= 0.0 || sys[ 1 ].first <= 0.0 || sys[ 2 ].first <= 0.0 )
00344     {
00345         res( 0, 0 ) = res( 1, 1 ) = res( 2, 2 ) = 1.0;
00346         return res;
00347     }
00348 
00349     // this implements the matrix product U * log( E ) * U.transposed()
00350     // note that u( i, j ) = jth value of the ith eigenvector
00351     res( 0, 0 ) = sys[ 0 ].second[ 0 ] * sys[ 0 ].second[ 0 ] * log( sys[ 0 ].first )
00352                 + sys[ 1 ].second[ 0 ] * sys[ 1 ].second[ 0 ] * log( sys[ 1 ].first )
00353                 + sys[ 2 ].second[ 0 ] * sys[ 2 ].second[ 0 ] * log( sys[ 2 ].first );
00354     res( 0, 1 ) = sys[ 0 ].second[ 0 ] * sys[ 0 ].second[ 1 ] * log( sys[ 0 ].first )
00355                 + sys[ 1 ].second[ 0 ] * sys[ 1 ].second[ 1 ] * log( sys[ 1 ].first )
00356                 + sys[ 2 ].second[ 0 ] * sys[ 2 ].second[ 1 ] * log( sys[ 2 ].first );
00357     res( 0, 2 ) = sys[ 0 ].second[ 0 ] * sys[ 0 ].second[ 2 ] * log( sys[ 0 ].first )
00358                 + sys[ 1 ].second[ 0 ] * sys[ 1 ].second[ 2 ] * log( sys[ 1 ].first )
00359                 + sys[ 2 ].second[ 0 ] * sys[ 2 ].second[ 2 ] * log( sys[ 2 ].first );
00360     res( 1, 1 ) = sys[ 0 ].second[ 1 ] * sys[ 0 ].second[ 1 ] * log( sys[ 0 ].first )
00361                 + sys[ 1 ].second[ 1 ] * sys[ 1 ].second[ 1 ] * log( sys[ 1 ].first )
00362                 + sys[ 2 ].second[ 1 ] * sys[ 2 ].second[ 1 ] * log( sys[ 2 ].first );
00363     res( 1, 2 ) = sys[ 0 ].second[ 1 ] * sys[ 0 ].second[ 2 ] * log( sys[ 0 ].first )
00364                 + sys[ 1 ].second[ 1 ] * sys[ 1 ].second[ 2 ] * log( sys[ 1 ].first )
00365                 + sys[ 2 ].second[ 1 ] * sys[ 2 ].second[ 2 ] * log( sys[ 2 ].first );
00366     res( 2, 2 ) = sys[ 0 ].second[ 2 ] * sys[ 0 ].second[ 2 ] * log( sys[ 0 ].first )
00367                 + sys[ 1 ].second[ 2 ] * sys[ 1 ].second[ 2 ] * log( sys[ 1 ].first )
00368                 + sys[ 2 ].second[ 2 ] * sys[ 2 ].second[ 2 ] * log( sys[ 2 ].first );
00369     return res;
00370 }
00371 
00372 /**
00373  * Calculate the exponential of the given real symmetric tensor.
00374  *
00375  * \param tens The tensor.
00376  * \return The exponential of the tensor.
00377  */
00378 template< typename T >
00379 WTensorSym< 2, 3, T > tensorExp( WTensorSym< 2, 3, T > const& tens )
00380 {
00381     WTensorSym< 2, 3, T > res;
00382 
00383     // calculate eigenvalues and eigenvectors
00384     RealEigenSystem sys;
00385     eigenEigenvector3D( tens, &sys );
00386 
00387     // this implements the matrix product U * exp( E ) * U.transposed()
00388     // note that u( i, j ) = jth value of the ith eigenvector
00389     res( 0, 0 ) = sys[ 0 ].second[ 0 ] * sys[ 0 ].second[ 0 ] * exp( sys[ 0 ].first )
00390                 + sys[ 1 ].second[ 0 ] * sys[ 1 ].second[ 0 ] * exp( sys[ 1 ].first )
00391                 + sys[ 2 ].second[ 0 ] * sys[ 2 ].second[ 0 ] * exp( sys[ 2 ].first );
00392     res( 0, 1 ) = sys[ 0 ].second[ 0 ] * sys[ 0 ].second[ 1 ] * exp( sys[ 0 ].first )
00393                 + sys[ 1 ].second[ 0 ] * sys[ 1 ].second[ 1 ] * exp( sys[ 1 ].first )
00394                 + sys[ 2 ].second[ 0 ] * sys[ 2 ].second[ 1 ] * exp( sys[ 2 ].first );
00395     res( 0, 2 ) = sys[ 0 ].second[ 0 ] * sys[ 0 ].second[ 2 ] * exp( sys[ 0 ].first )
00396                 + sys[ 1 ].second[ 0 ] * sys[ 1 ].second[ 2 ] * exp( sys[ 1 ].first )
00397                 + sys[ 2 ].second[ 0 ] * sys[ 2 ].second[ 2 ] * exp( sys[ 2 ].first );
00398     res( 1, 1 ) = sys[ 0 ].second[ 1 ] * sys[ 0 ].second[ 1 ] * exp( sys[ 0 ].first )
00399                 + sys[ 1 ].second[ 1 ] * sys[ 1 ].second[ 1 ] * exp( sys[ 1 ].first )
00400                 + sys[ 2 ].second[ 1 ] * sys[ 2 ].second[ 1 ] * exp( sys[ 2 ].first );
00401     res( 1, 2 ) = sys[ 0 ].second[ 1 ] * sys[ 0 ].second[ 2 ] * exp( sys[ 0 ].first )
00402                 + sys[ 1 ].second[ 1 ] * sys[ 1 ].second[ 2 ] * exp( sys[ 1 ].first )
00403                 + sys[ 2 ].second[ 1 ] * sys[ 2 ].second[ 2 ] * exp( sys[ 2 ].first );
00404     res( 2, 2 ) = sys[ 0 ].second[ 2 ] * sys[ 0 ].second[ 2 ] * exp( sys[ 0 ].first )
00405                 + sys[ 1 ].second[ 2 ] * sys[ 1 ].second[ 2 ] * exp( sys[ 1 ].first )
00406                 + sys[ 2 ].second[ 2 ] * sys[ 2 ].second[ 2 ] * exp( sys[ 2 ].first );
00407     return res;
00408 }
00409 
00410 /**
00411  * Find the maxima of a spherical function represented by a symmetric tensor.
00412  *
00413  *
00414  *
00415  */
00416 template< std::size_t order, typename Data_T >
00417 void findSymmetricSphericalFunctionMaxima( WTensorSym< order, 3, Data_T > const& tensor,
00418                                            double threshold, double minAngularSeparationCosine, double stepSize,
00419                                            std::vector< WVector3d > const& startingPoints,
00420                                            std::vector< WVector3d >& maxima ) //  NOLINT
00421 {
00422     std::vector< double > values;
00423 
00424     std::vector< WVector3d >::const_iterator it;
00425     for( it = startingPoints.begin(); it != startingPoints.end(); ++it )
00426     {
00427         WVector3d position = *it;
00428         WVector3d gradient = approximateGradient( tensor, position );
00429         int iter;
00430 
00431         for( iter = 0; iter < 100; ++iter )
00432         {
00433             WVector3d newPosition = position + stepSize * gradient;
00434             normalize( newPosition );
00435             WVector3d newGradient = approximateGradient( tensor, newPosition );
00436 
00437             double cos = dot( newGradient, gradient );
00438             if( cos > 0.985 ) // less then 10 degree
00439             {
00440                 stepSize *= 2.0;
00441             }
00442             else if( cos < 0.866 ) // more than 30 degree
00443             {
00444                 stepSize *= 0.5;
00445                 if( stepSize < 0.000001 )
00446                 {
00447                     break;
00448                 }
00449             }
00450             if( cos > 0.886 ) // less than 30 degree
00451             {
00452                 gradient = newGradient;
00453                 position = newPosition;
00454             }
00455         }
00456         if( iter != 100 )
00457         {
00458             double newFuncValue = tensor.evaluateSphericalFunction( position );
00459 
00460             bool add = true;
00461 
00462             std::vector< int > m;
00463             m.reserve( maxima.size() );
00464 
00465             for( std::size_t k = 0; k != maxima.size(); ++k )
00466             {
00467                 // find all maxima that are to close to this one
00468                 if( dot( position, maxima[ k ] ) > minAngularSeparationCosine
00469                  || dot( maxima[ k ], -1.0 * position ) > minAngularSeparationCosine )
00470                 {
00471                     m.push_back( k );
00472                     if( values[ k ] >= newFuncValue )
00473                     {
00474                         add = false;
00475                     }
00476                 }
00477             }
00478             if( add )
00479             {
00480                 maxima.push_back( position );
00481                 values.push_back( newFuncValue );
00482 
00483                 for( int k = static_cast< int >( m.size() - 1 ); k > 0; --k )
00484                 {
00485                     maxima.erase( maxima.begin() + m[ k ] );
00486                     values.erase( values.begin() + m[ k ] );
00487                 }
00488             }
00489         }
00490     }
00491 
00492     // remove maxima that are too small
00493     double max = 0;
00494     for( std::size_t k = 0; k != maxima.size(); ++k )
00495     {
00496         if( values[ k ] > max )
00497         {
00498             max = values[ k ];
00499         }
00500     }
00501 
00502     std::size_t k = 0;
00503     while( k < maxima.size() )
00504     {
00505         if( values[ k ] < threshold * max )
00506         {
00507             maxima.erase( maxima.begin() + k );
00508             values.erase( values.begin() + k );
00509         }
00510         else
00511         {
00512             ++k;
00513         }
00514     }
00515 }
00516 
00517 template< std::size_t order, typename Data_T >
00518 WVector3d approximateGradient( WTensorSym< order, 3, Data_T > const& tensor, WVector3d const& pos )
00519 {
00520     WVector3d eXY;
00521     WVector3d eZ;
00522 
00523     if( fabs( fabs( pos[ 2 ] ) - 1.0 ) < 0.001 )
00524     {
00525         eXY = WVector3d( 1.0, 0.0, 0.0 );
00526         eZ = WVector3d( 0.0, 1.0, 0.0 );
00527     }
00528     else
00529     {
00530         // this is vectorProduct( z, pos )
00531         eXY = WVector3d( -pos[ 1 ], pos[ 0 ], 0.0 );
00532         normalize( eXY );
00533         // this is vectorProduct( eXY, pos )
00534         eZ = WVector3d( eXY[ 1 ] * pos[ 2 ], -eXY[ 0 ] * pos[ 2 ], eXY[ 0 ] * pos[ 1 ] - eXY[ 1 ] * pos[ 0 ] );
00535         normalize( eZ );
00536     }
00537 
00538     double dXY = ( tensor.evaluateSphericalFunction( normalize( pos + eXY * 0.0001 ) )
00539                  - tensor.evaluateSphericalFunction( normalize( pos - eXY * 0.0001 ) ) )
00540                  / 0.0002;
00541     double dZ = ( tensor.evaluateSphericalFunction( normalize( pos + eZ * 0.0001 ) )
00542                 - tensor.evaluateSphericalFunction( normalize( pos - eZ * 0.0001 ) ) )
00543                 / 0.0002;
00544 
00545     // std::sqrt( 1.0 - z² ) = sin( acos( z ) ) = sin( theta ) in spherical coordinates
00546     double d = 1.0 - pos[ 2 ] * pos[ 2 ];
00547     if( d < 0.0 ) // avoid possible numerical problems
00548     {
00549         d = 0.0;
00550     }
00551     WVector3d res = eZ * dZ + eXY * ( dXY / std::sqrt( d ) );
00552     normalize( res );
00553     return res;
00554 }
00555 
00556 #endif  // WTENSORFUNCTIONS_H