OpenWalnut
1.4.0
|
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 }