OpenWalnut  1.2.5
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
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/WLinearAlgebra.h"
41 
42 WLine::WLine( const std::vector< WPosition > &points )
43  : WMixinVector< WPosition >( points )
44 {
45 }
46 
48  : WMixinVector< WPosition >()
49 {
50 }
51 
52 const WPosition& midPoint( const WLine& line )
53 {
54  if( line.empty() )
55  {
56  throw WOutOfBounds( std::string( "There is no midpoint for an empty line." ) );
57  }
58  return line[( line.size() - 1 ) / 2];
59 }
60 
62 {
63  std::reverse( begin(), end() );
64 }
65 
66 double pathLength( const WLine& line )
67 {
68  double len = 0;
69  // incase of size() <= 1 the for loop will not run!
70  for( size_t i = 1; i < line.size(); ++i )
71  {
72  len += length( line[i - 1] - line[i] );
73  }
74  return len;
75 }
76 
77 void WLine::resampleByNumberOfPoints( size_t numPoints )
78 {
79  WLine newLine;
80  newLine.reserve( numPoints );
81  if( size() != numPoints && size() > 1 && numPoints > 0 )
82  {
83  const double pathL = pathLength( *this );
84  double newSegmentLength = pathL / ( numPoints - 1 );
85  const double delta = newSegmentLength * 1.0e-10; // 1.0e-10 which represents the precision is choosen by intuition
86  double remainingLength = 0.0;
87  newLine.push_back( front() );
88  for( size_t i = 0; i < ( size() - 1 ); ++i )
89  {
90  remainingLength += length( at( i ) - at( i + 1 ) );
91  while( ( remainingLength > newSegmentLength ) || std::abs( remainingLength - newSegmentLength ) < delta )
92  {
93  remainingLength -= newSegmentLength;
94  // TODO(math): fix numerical issuses: newSegmentLength may be wrong => great offset by many intraSegment sample points
95  // remainingLength may be wrong => ...
96  // Take a look at the unit test testNumericalStabilityOfResampling
97  WPosition newPoint = at( i + 1 ) + remainingLength * normalize( at( i ) - at( i + 1 ) );
98  newLine.push_back( newPoint );
99  // std::cout << "line size so far" << newLine.size() << " lenght so far: " << newLine.pathLength() << std::endl;
100  // std::cout << numPoints - newLine.size() << std::endl;
101  }
102  }
103  // using string_utils::operator<<;
104  // std::cout << "this: " << *this << std::endl << "new: " << newLine << std::endl;
105  // std::cout << "|remL - newSegL|: " << std::abs( remainingLength - newSegmentLength ) << std::endl;
106  // std::cout << std::setprecision( 35 ) << "remainingLength: " << remainingLength << " newSegmentLength: " << newSegmentLength << std::endl;
107  // std::cout << "this size: " << size() << " new size: " << newLine.size() << std::endl;
108  }
109  else if( size() == 1 && size() < numPoints )
110  {
111  for( size_t i = 0; i < numPoints; ++i )
112  {
113  newLine.push_back( front() );
114  }
115  }
116  if( size() != numPoints )
117  {
118  this->WMixinVector< WPosition >::operator=( newLine );
119  }
120  // Note if the size() == 0, then the resampled tract is also of length 0
121 }
122 
124 {
125  if( empty() )
126  {
127  return;
128  }
129 
130  // Note: We cannot use std::remove for that since it allows only unary predicates to identify
131  // elements which are to be removed
132  WLine newLine;
133  newLine.reserve( size() );
134  newLine.push_back( front() );
135  for( const_iterator cit = begin()++; cit != end(); ++cit )
136  {
137  if( length( *cit - newLine.back() ) > wlimits::DBL_EPS )
138  {
139  newLine.push_back( *cit );
140  }
141  }
142  this->WMixinVector< WPosition >::operator=( newLine );
143 }
144 
145 void WLine::resampleBySegmentLength( double newSegmentLength )
146 {
147  // eliminate duplicate points following next to another
149 
150  if( empty() || size() == 1 )
151  {
152  return;
153  }
154  WLine newLine;
155  newLine.push_back( front() );
156  for( size_t i = 1; i < size(); )
157  {
158  if( length( newLine.back() - ( *this )[i] ) > newSegmentLength )
159  {
160  const WPosition& pred = ( *this )[i - 1];
161  if( pred == newLine.back() )
162  {
163  // Then there is no triangle and the old Segment Length is bigger as the new segment
164  // length
165  newLine.push_back( newLine.back() + normalize( ( *this )[i] - pred ) * newSegmentLength );
166  continue;
167  }
168  else // this is the general case, and the point we search is inbetween the pred and the current position
169  {
170  // we compute the three coefficents describing the quadradic equation of the point of intersection of
171  // the circle with radius newSegmentLength and the segmend: pred and ( *this )[i].
172  // alpha * x^2 + beta * x + gamma = 0
173  double alpha = ( ( *this )[i][0] - pred[0] ) * ( ( *this )[i][0] - pred[0] ) +
174  ( ( *this )[i][1] - pred[1] ) * ( ( *this )[i][1] - pred[1] ) +
175  ( ( *this )[i][2] - pred[2] ) * ( ( *this )[i][2] - pred[2] );
176 
177  double beta = 2.0 * ( ( *this )[i][0] - pred[0] ) * ( pred[0] - newLine.back()[0] ) +
178  2.0 * ( ( *this )[i][1] - pred[1] ) * ( pred[1] - newLine.back()[1] ) +
179  2.0 * ( ( *this )[i][2] - pred[2] ) * ( pred[2] - newLine.back()[2] );
180 
181  double gamma = ( pred[0] - newLine.back()[0] ) * ( pred[0] - newLine.back()[0] ) +
182  ( pred[1] - newLine.back()[1] ) * ( pred[1] - newLine.back()[1] ) +
183  ( pred[2] - newLine.back()[2] ) * ( pred[2] - newLine.back()[2] ) - newSegmentLength * newSegmentLength;
184 
185  typedef std::pair< std::complex< double >, std::complex< double > > ComplexPair;
186  ComplexPair solution = solveRealQuadraticEquation( alpha, beta, gamma );
187  // NOTE: if those asserts fire, then this algo is wrong and produces wrong results, and I've to search to bug!
188  WAssert( std::imag( solution.first ) == 0.0, "Invalid quadratic equation while computing resamplingBySegmentLength" );
189  WAssert( std::imag( solution.second ) == 0.0, "Invalid quadratic equation while computing resamplingBySegmentLength" );
190  WPosition pointOfIntersection;
191  if( std::real( solution.first ) > 0.0 )
192  {
193  pointOfIntersection = pred + std::real( solution.first ) * ( ( *this )[i] - pred );
194  }
195  else
196  {
197  pointOfIntersection = pred + std::real( solution.second ) * ( ( *this )[i] - pred );
198  }
199  newLine.push_back( pointOfIntersection );
200  }
201  }
202  ++i;
203  }
204  if( length( newLine.back() - ( *this )[size() - 1] ) > newSegmentLength / 2.0 )
205  {
206  WVector3d direction = normalize( ( *this )[size() - 1] - newLine.back() );
207  newLine.push_back( newLine.back() + direction * newSegmentLength );
208  }
209  this->WMixinVector< WPosition >::operator=( newLine );
210 }
211 
212 int equalsDelta( const WLine& line, const WLine& other, double delta )
213 {
214  size_t pts = ( std::min )( other.size(), line.size() ); // This ( std::min ) thing compiles also under Win32/Win64
215  size_t diffPos = 0;
216  bool sameLines = true;
217  for( diffPos = 0; ( diffPos < pts ) && sameLines; ++diffPos )
218  {
219  for( int x = 0; x < 3; ++x ) // since WLine uses WPosition as elements there are 3 components per position
220  {
221  sameLines = sameLines && ( std::abs( line[diffPos][x] - other[diffPos][x] ) <= delta );
222  }
223  }
224  if( sameLines && ( line.size() == other.size() ) )
225  {
226  return -1;
227  }
228  if( !sameLines )
229  {
230  return diffPos - 1;
231  }
232  return diffPos;
233 }
234 
235 double maxSegmentLength( const WLine& line )
236 {
237  double result = 0.0;
238  if( line.empty() || line.size() == 1 )
239  {
240  return result;
241  }
242  for( size_t i = 0; i < line.size() - 1; ++i )
243  {
244  result = std::max( result, static_cast< double >( length( line[i] - line[i+1] ) ) );
245  }
246  return result;
247 }
248 
249 void WLine::unifyDirectionBy( const WLine& other )
250 {
251  const size_t numBasePoints = 4;
252  boost::array< WPosition, numBasePoints > m;
253  boost::array< WPosition, numBasePoints > n;
254 
255  double distance = 0.0;
256  double inverseDistance = 0.0;
257  for( size_t i = 0; i < numBasePoints; ++i )
258  {
259  m[i] = other.at( ( other.size() - 1 ) * static_cast< double >( i ) / ( numBasePoints - 1 ) );
260  n[i] = at( ( size() - 1 ) * static_cast< double >( i ) / ( numBasePoints - 1 ) );
261  distance += length2( m[i] - n[i] );
262  inverseDistance += length2( m[i] - at( ( size() - 1 ) * static_cast< double >( numBasePoints - 1 - i ) / ( numBasePoints - 1 ) ) );
263  }
264  distance /= static_cast< double >( numBasePoints );
265  inverseDistance /= static_cast< double >( numBasePoints );
266 
267  if( inverseDistance < distance )
268  {
269  this->reverseOrder();
270  }
271 }
272 
273 WBoundingBox computeBoundingBox( const WLine& line )
274 {
275  WBoundingBox result;
276  for( WLine::const_iterator cit = line.begin(); cit != line.end(); ++cit )
277  {
278  result.expandBy( *cit );
279  }
280  return result;
281 }