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