00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
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
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;
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
00095
00096
00097 WPosition newPoint = at( i + 1 ) + remainingLength * normalize( at( i ) - at( i + 1 ) );
00098 newLine.push_back( newPoint );
00099
00100
00101 }
00102 }
00103
00104
00105
00106
00107
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
00121 }
00122
00123 void WLine::removeAdjacentDuplicates()
00124 {
00125 if( empty() )
00126 {
00127 return;
00128 }
00129
00130
00131
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
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
00164
00165 newLine.push_back( newLine.back() + normalize( ( *this )[i] - pred ) * newSegmentLength );
00166 continue;
00167 }
00168 else
00169 {
00170
00171
00172
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
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() );
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 )
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 }