OpenWalnut  1.4.0
WLine.cpp
1 //---------------------------------------------------------------------------
2 //
3 // Project: OpenWalnut ( http://www.openwalnut.org )
4 //
5 // Copyright 2009 OpenWalnut Community, BSV@Uni-Leipzig and CNCF@MPI-CBS
6 // For more information see http://www.openwalnut.org/copying
7 //
8 // This file is part of OpenWalnut.
9 //
10 // OpenWalnut is free software: you can redistribute it and/or modify
11 // it under the terms of the GNU Lesser General Public License as published by
12 // the Free Software Foundation, either version 3 of the License, or
13 // (at your option) any later version.
14 //
15 // OpenWalnut is distributed in the hope that it will be useful,
16 // but WITHOUT ANY WARRANTY; without even the implied warranty of
17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 // GNU Lesser General Public License for more details.
19 //
20 // You should have received a copy of the GNU Lesser General Public License
21 // along with OpenWalnut. If not, see <http://www.gnu.org/licenses/>.
22 //
23 //---------------------------------------------------------------------------
24 
25 #include <algorithm>
26 #include <complex>
27 #include <iostream>
28 #include <string>
29 #include <utility>
30 #include <vector>
31 
32 #include <boost/array.hpp>
33 
34 #include "../exceptions/WOutOfBounds.h"
35 #include "../WAssert.h"
36 #include "../WLimits.h"
37 #include "../WStringUtils.h"
38 #include "WLine.h"
39 #include "WPolynomialEquationSolvers.h"
40 #include "linearAlgebra/WPosition.h"
41 #include "linearAlgebra/WVectorFixed.h"
42 
43 WLine::WLine( const std::vector< WPosition > &points )
44  : WMixinVector< WPosition >( points )
45 {
46 }
47 
49  : WMixinVector< WPosition >()
50 {
51 }
52 
53 const WPosition& midPoint( const WLine& line )
54 {
55  if( line.empty() )
56  {
57  throw WOutOfBounds( std::string( "There is no midpoint for an empty line." ) );
58  }
59  return line[( line.size() - 1 ) / 2];
60 }
61 
63 {
64  std::reverse( begin(), end() );
65 }
66 
67 double pathLength( const WLine& line )
68 {
69  double len = 0;
70  // incase of size() <= 1 the for loop will not run!
71  for( size_t i = 1; i < line.size(); ++i )
72  {
73  len += length( line[i - 1] - line[i] );
74  }
75  return len;
76 }
77 
78 void WLine::resampleByNumberOfPoints( size_t numPoints )
79 {
80  WLine newLine;
81  newLine.reserve( numPoints );
82  if( size() != numPoints && size() > 1 && numPoints > 0 )
83  {
84  const double pathL = pathLength( *this );
85  double newSegmentLength = pathL / ( numPoints - 1 );
86  const double delta = newSegmentLength * 1.0e-10; // 1.0e-10 which represents the precision is choosen by intuition
87  double remainingLength = 0.0;
88  newLine.push_back( front() );
89  for( size_t i = 0; i < ( size() - 1 ); ++i )
90  {
91  remainingLength += length( at( i ) - at( i + 1 ) );
92  while( ( remainingLength > newSegmentLength ) || std::abs( remainingLength - newSegmentLength ) < delta )
93  {
94  remainingLength -= newSegmentLength;
95  // TODO(math): fix numerical issuses: newSegmentLength may be wrong => great offset by many intraSegment sample points
96  // remainingLength may be wrong => ...
97  // Take a look at the unit test testNumericalStabilityOfResampling
98  WPosition newPoint = at( i + 1 ) + remainingLength * normalize( at( i ) - at( i + 1 ) );
99  newLine.push_back( newPoint );
100  // std::cout << "line size so far" << newLine.size() << " lenght so far: " << newLine.pathLength() << std::endl;
101  // std::cout << numPoints - newLine.size() << std::endl;
102  }
103  }
104  // using string_utils::operator<<;
105  // std::cout << "this: " << *this << std::endl << "new: " << newLine << std::endl;
106  // std::cout << "|remL - newSegL|: " << std::abs( remainingLength - newSegmentLength ) << std::endl;
107  // std::cout << std::setprecision( 35 ) << "remainingLength: " << remainingLength << " newSegmentLength: " << newSegmentLength << std::endl;
108  // std::cout << "this size: " << size() << " new size: " << newLine.size() << std::endl;
109  }
110  else if( size() == 1 && size() < numPoints )
111  {
112  for( size_t i = 0; i < numPoints; ++i )
113  {
114  newLine.push_back( front() );
115  }
116  }
117  if( size() != numPoints )
118  {
119  this->WMixinVector< WPosition >::operator=( newLine );
120  }
121  // Note if the size() == 0, then the resampled tract is also of length 0
122 }
123 
125 {
126  if( empty() )
127  {
128  return;
129  }
130 
131  // Note: We cannot use std::remove for that since it allows only unary predicates to identify
132  // elements which are to be removed
133  WLine newLine;
134  newLine.reserve( size() );
135  newLine.push_back( front() );
136 
137  for( size_t i = 1; i < size(); ++i )
138  {
139  if( length( (*this)[i] - newLine.back() ) > wlimits::DBL_EPS )
140  {
141  newLine.push_back( (*this)[i] );
142  }
143  }
144  this->WMixinVector< WPosition >::operator=( newLine );
145 }
146 
147 void WLine::resampleBySegmentLength( double newSegmentLength )
148 {
149  // eliminate duplicate points following next to another
151 
152  if( empty() || size() == 1 )
153  {
154  return;
155  }
156  WLine newLine;
157  newLine.push_back( front() );
158  for( size_t i = 1; i < size(); )
159  {
160  WPosition current = (*this)[i];
161  if( length( newLine.back() - current ) > newSegmentLength )
162  {
163  newLine.push_back( newLine.back() + normalize( ( current - newLine.back() ) * newSegmentLength ) );
164  continue;
165  }
166  else
167  {
168  do
169  {
170  i = i + 1;
171  current = (*this)[i];
172  }
173  while( length( newLine.back() - current ) < newSegmentLength && i < size() );
174 
175  if( i >= size() ) // discard last point, as we dont want to elongate the new line
176  {
177  break;
178  }
179  const WPosition pred = ( *this )[ i - 1 ];
180 
181  WVector3d lineDirection = current - pred;
182  WAssert( lineDirection != WVector3d( 0.0, 0.0, 0.0 ), "current should be diffrent from pred" );
183  WVector3d o_c = pred - newLine.back(); // origin - center
184  double alpha = dot( lineDirection, lineDirection );
185  double beta = 2.0 * dot( lineDirection, o_c );
186  double gamma = dot( o_c, o_c ) - newSegmentLength * newSegmentLength;
187 
188  std::pair< std::complex< double >, std::complex< double > > solution = solveRealQuadraticEquation( alpha, beta, gamma );
189  // NOTE: if this assert fires, then this algo is wrong and produces wrong results, and I've to search to bug!
190  WAssert( std::imag( solution.first ) == 0.0 && std::imag( solution.second ) == 0.0, "Imaginary solution detected." );
191  WPosition pointOfIntersection;
192  if( std::real( solution.first ) > 0.0 )
193  {
194  pointOfIntersection = pred + std::real( solution.first ) * ( ( *this )[i] - pred );
195  }
196  else
197  {
198  pointOfIntersection = pred + std::real( solution.second ) * ( ( *this )[i] - pred );
199  }
200  newLine.push_back( pointOfIntersection );
201  }
202  }
203  this->WMixinVector< WPosition >::operator=( newLine );
204 }
205 
206 int equalsDelta( const WLine& line, const WLine& other, double delta )
207 {
208  size_t pts = ( std::min )( other.size(), line.size() ); // This ( std::min ) thing compiles also under Win32/Win64
209  size_t diffPos = 0;
210  bool sameLines = true;
211  for( diffPos = 0; ( diffPos < pts ) && sameLines; ++diffPos )
212  {
213  for( int x = 0; x < 3; ++x ) // since WLine uses WPosition as elements there are 3 components per position
214  {
215  sameLines = sameLines && ( std::abs( line[diffPos][x] - other[diffPos][x] ) <= delta );
216  }
217  }
218  if( sameLines && ( line.size() == other.size() ) )
219  {
220  return -1;
221  }
222  if( !sameLines )
223  {
224  return diffPos - 1;
225  }
226  return diffPos;
227 }
228 
229 double maxSegmentLength( const WLine& line )
230 {
231  double result = 0.0;
232  if( line.empty() || line.size() == 1 )
233  {
234  return result;
235  }
236  for( size_t i = 0; i < line.size() - 1; ++i )
237  {
238  result = std::max( result, static_cast< double >( length( line[i] - line[i+1] ) ) );
239  }
240  return result;
241 }
242 
243 void WLine::unifyDirectionBy( const WLine& other )
244 {
245  const size_t numBasePoints = 4;
246  boost::array< WPosition, numBasePoints > m;
247  boost::array< WPosition, numBasePoints > n;
248 
249  double distance = 0.0;
250  double inverseDistance = 0.0;
251  for( size_t i = 0; i < numBasePoints; ++i )
252  {
253  m[i] = other.at( ( other.size() - 1 ) * static_cast< double >( i ) / ( numBasePoints - 1 ) );
254  n[i] = at( ( size() - 1 ) * static_cast< double >( i ) / ( numBasePoints - 1 ) );
255  distance += length2( m[i] - n[i] );
256  inverseDistance += length2( m[i] - at( ( size() - 1 ) * static_cast< double >( numBasePoints - 1 - i ) / ( numBasePoints - 1 ) ) );
257  }
258  distance /= static_cast< double >( numBasePoints );
259  inverseDistance /= static_cast< double >( numBasePoints );
260 
261  if( inverseDistance < distance )
262  {
263  this->reverseOrder();
264  }
265 }
266 
267 WBoundingBox computeBoundingBox( const WLine& line )
268 {
269  WBoundingBox result;
270  for( WLine::const_iterator cit = line.begin(); cit != line.end(); ++cit )
271  {
272  result.expandBy( *cit );
273  }
274  return result;
275 }