OpenWalnut  1.4.0
WLine.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 <algorithm>
00026 #include <complex>
00027 #include <iostream>
00028 #include <string>
00029 #include <utility>
00030 #include <vector>
00031 
00032 #include <boost/array.hpp>
00033 
00034 #include "../exceptions/WOutOfBounds.h"
00035 #include "../WAssert.h"
00036 #include "../WLimits.h"
00037 #include "../WStringUtils.h"
00038 #include "WLine.h"
00039 #include "WPolynomialEquationSolvers.h"
00040 #include "linearAlgebra/WPosition.h"
00041 #include "linearAlgebra/WVectorFixed.h"
00042 
00043 WLine::WLine( const std::vector< WPosition > &points )
00044     : WMixinVector< WPosition >( points )
00045 {
00046 }
00047 
00048 WLine::WLine()
00049     : WMixinVector< WPosition >()
00050 {
00051 }
00052 
00053 const WPosition& midPoint( const WLine& line )
00054 {
00055     if( line.empty() )
00056     {
00057         throw WOutOfBounds( std::string( "There is no midpoint for an empty line." ) );
00058     }
00059     return line[( line.size() - 1 ) / 2];
00060 }
00061 
00062 void WLine::reverseOrder()
00063 {
00064     std::reverse( begin(), end() );
00065 }
00066 
00067 double pathLength( const WLine& line )
00068 {
00069     double len = 0;
00070     // incase of size() <= 1 the for loop will not run!
00071     for( size_t i = 1; i < line.size(); ++i )
00072     {
00073         len += length( line[i - 1] - line[i] );
00074     }
00075     return len;
00076 }
00077 
00078 void WLine::resampleByNumberOfPoints( size_t numPoints )
00079 {
00080     WLine newLine;
00081     newLine.reserve( numPoints );
00082     if( size() != numPoints && size() > 1 && numPoints > 0 )
00083     {
00084         const double pathL = pathLength( *this );
00085         double newSegmentLength = pathL / ( numPoints - 1 );
00086         const double delta = newSegmentLength * 1.0e-10; // 1.0e-10 which represents the precision is choosen by intuition
00087         double remainingLength = 0.0;
00088         newLine.push_back( front() );
00089         for( size_t i = 0; i < ( size() - 1 ); ++i )
00090         {
00091             remainingLength += length( at( i ) - at( i + 1 ) );
00092             while( ( remainingLength > newSegmentLength ) || std::abs( remainingLength - newSegmentLength ) < delta )
00093             {
00094                 remainingLength -= newSegmentLength;
00095                 // TODO(math): fix numerical issuses: newSegmentLength may be wrong => great offset by many intraSegment sample points
00096                 //                                    remainingLength may be wrong => ...
00097                 //                                    Take a look at the unit test testNumericalStabilityOfResampling
00098                 WPosition newPoint = at( i + 1 ) + remainingLength * normalize( at( i ) - at( i + 1 ) );
00099                 newLine.push_back( newPoint );
00100                 // std::cout << "line size so far" << newLine.size() << " lenght so far: " << newLine.pathLength() << std::endl;
00101                 // std::cout << numPoints - newLine.size() << std::endl;
00102             }
00103         }
00104         // using string_utils::operator<<;
00105         // std::cout << "this: " << *this << std::endl << "new:  " << newLine << std::endl;
00106         // std::cout << "|remL - newSegL|: " << std::abs( remainingLength - newSegmentLength ) << std::endl;
00107         // std::cout << std::setprecision( 35 ) << "remainingLength: " << remainingLength << " newSegmentLength: " << newSegmentLength << std::endl;
00108         // std::cout << "this size: " << size() << " new size: " << newLine.size() << std::endl;
00109     }
00110     else if( size() == 1 && size() < numPoints )
00111     {
00112         for( size_t i = 0; i < numPoints; ++i )
00113         {
00114             newLine.push_back( front() );
00115         }
00116     }
00117     if( size() != numPoints )
00118     {
00119         this->WMixinVector< WPosition >::operator=( newLine );
00120     }
00121     // Note if the size() == 0, then the resampled tract is also of length 0
00122 }
00123 
00124 void WLine::removeAdjacentDuplicates()
00125 {
00126     if( empty() )
00127     {
00128         return;
00129     }
00130 
00131     // Note: We cannot use std::remove for that since it allows only unary predicates to identify
00132     // elements which are to be removed
00133     WLine newLine;
00134     newLine.reserve( size() );
00135     newLine.push_back( front() );
00136 
00137     for( size_t i = 1; i < size(); ++i )
00138     {
00139         if( length( (*this)[i] - newLine.back() ) > wlimits::DBL_EPS )
00140         {
00141             newLine.push_back( (*this)[i] );
00142         }
00143     }
00144     this->WMixinVector< WPosition >::operator=( newLine );
00145 }
00146 
00147 void WLine::resampleBySegmentLength( double newSegmentLength )
00148 {
00149     // eliminate duplicate points following next to another
00150     removeAdjacentDuplicates();
00151 
00152     if( empty() || size() == 1 )
00153     {
00154         return;
00155     }
00156     WLine newLine;
00157     newLine.push_back( front() );
00158     for( size_t i = 1; i < size(); )
00159     {
00160         WPosition current = (*this)[i];
00161         if( length( newLine.back() - current ) > newSegmentLength )
00162         {
00163             newLine.push_back( newLine.back() + normalize( ( current - newLine.back() ) * newSegmentLength ) );
00164             continue;
00165         }
00166         else
00167         {
00168             do
00169             {
00170                 i = i + 1;
00171                 current = (*this)[i];
00172             }
00173             while( length( newLine.back() - current ) < newSegmentLength && i < size() );
00174 
00175             if( i >= size() ) // discard last point, as we dont want to elongate the new line
00176             {
00177                 break;
00178             }
00179             const WPosition pred = ( *this )[ i - 1 ];
00180 
00181             WVector3d lineDirection = current - pred;
00182             WAssert( lineDirection != WVector3d( 0.0, 0.0, 0.0 ), "current should be diffrent from pred" );
00183             WVector3d o_c = pred - newLine.back(); // origin - center
00184             double alpha = dot( lineDirection, lineDirection );
00185             double beta = 2.0 * dot( lineDirection, o_c );
00186             double gamma = dot( o_c, o_c ) - newSegmentLength * newSegmentLength;
00187 
00188             std::pair< std::complex< double >, std::complex< double > > solution = solveRealQuadraticEquation( alpha, beta, gamma );
00189             // NOTE: if this assert fires, then this algo is wrong and produces wrong results, and I've to search to bug!
00190             WAssert( std::imag( solution.first ) == 0.0 && std::imag( solution.second ) == 0.0, "Imaginary solution detected." );
00191             WPosition pointOfIntersection;
00192             if( std::real( solution.first ) > 0.0 )
00193             {
00194                 pointOfIntersection = pred + std::real( solution.first ) * ( ( *this )[i] - pred );
00195             }
00196             else
00197             {
00198                 pointOfIntersection = pred + std::real( solution.second ) * ( ( *this )[i] - pred );
00199             }
00200             newLine.push_back( pointOfIntersection );
00201         }
00202     }
00203     this->WMixinVector< WPosition >::operator=( newLine );
00204 }
00205 
00206 int equalsDelta( const WLine& line, const WLine& other, double delta )
00207 {
00208     size_t pts = ( std::min )( other.size(), line.size() ); // This ( std::min ) thing compiles also under Win32/Win64
00209     size_t diffPos = 0;
00210     bool sameLines = true;
00211     for( diffPos = 0; ( diffPos < pts ) && sameLines; ++diffPos )
00212     {
00213         for( int x = 0; x < 3; ++x ) // since WLine uses WPosition as elements there are 3 components per position
00214         {
00215             sameLines = sameLines && ( std::abs( line[diffPos][x] - other[diffPos][x] ) <= delta );
00216         }
00217     }
00218     if( sameLines && ( line.size() == other.size() ) )
00219     {
00220         return -1;
00221     }
00222     if( !sameLines )
00223     {
00224         return diffPos - 1;
00225     }
00226     return diffPos;
00227 }
00228 
00229 double maxSegmentLength( const WLine& line )
00230 {
00231     double result = 0.0;
00232     if( line.empty() || line.size() == 1 )
00233     {
00234         return result;
00235     }
00236     for( size_t i = 0; i < line.size() - 1; ++i )
00237     {
00238         result = std::max( result, static_cast< double >( length( line[i] - line[i+1] ) ) );
00239     }
00240     return result;
00241 }
00242 
00243 void WLine::unifyDirectionBy( const WLine& other )
00244 {
00245     const size_t numBasePoints = 4;
00246     boost::array< WPosition, numBasePoints > m;
00247     boost::array< WPosition, numBasePoints > n;
00248 
00249     double distance = 0.0;
00250     double inverseDistance = 0.0;
00251     for( size_t i = 0; i < numBasePoints; ++i )
00252     {
00253         m[i] = other.at( ( other.size() - 1 ) * static_cast< double >( i ) / ( numBasePoints - 1 ) );
00254         n[i] = at( ( size() - 1 ) * static_cast< double >( i ) / ( numBasePoints - 1 ) );
00255         distance += length2( m[i] - n[i] );
00256         inverseDistance += length2( m[i] - at( ( size() - 1 ) * static_cast< double >( numBasePoints - 1 - i ) / ( numBasePoints - 1 ) ) );
00257     }
00258     distance /= static_cast< double >( numBasePoints );
00259     inverseDistance /= static_cast< double >( numBasePoints );
00260 
00261     if( inverseDistance < distance )
00262     {
00263         this->reverseOrder();
00264     }
00265 }
00266 
00267 WBoundingBox computeBoundingBox( const WLine& line )
00268 {
00269     WBoundingBox result;
00270     for( WLine::const_iterator cit = line.begin(); cit != line.end(); ++cit )
00271     {
00272         result.expandBy( *cit );
00273     }
00274     return result;
00275 }