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