DGtal  1.3.beta
TangencyComputer.ih
1 /**
2  * This program is free software: you can redistribute it and/or modify
3  * it under the terms of the GNU Lesser General Public License as
4  * published by the Free Software Foundation, either version 3 of the
5  * License, or (at your option) any later version.
6  *
7  * This program is distributed in the hope that it will be useful,
8  * but WITHOUT ANY WARRANTY; without even the implied warranty of
9  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10  * GNU General Public License for more details.
11  *
12  * You should have received a copy of the GNU General Public License
13  * along with this program. If not, see <http://www.gnu.org/licenses/>.
14  *
15  **/
16 
17 /**
18  * @file TangencyComputer.ih
19  * @author Jacques-Olivier Lachaud (\c jacques-olivier.lachaud@univ-savoie.fr )
20  * Laboratory of Mathematics (CNRS, UMR 5127), University of Savoie, France
21  *
22  * @date 2021/07/16
23  *
24  * Implementation of inline methods defined in TangencyComputer.h
25  *
26  * This file is part of the DGtal library.
27  */
28 
29 
30 //////////////////////////////////////////////////////////////////////////////
31 #include <cstdlib>
32 //////////////////////////////////////////////////////////////////////////////
33 
34 ///////////////////////////////////////////////////////////////////////////////
35 // class TangencyComputer
36 ///////////////////////////////////////////////////////////////////////////////
37 
38 //-----------------------------------------------------------------------------
39 template <typename TKSpace>
40 DGtal::TangencyComputer<TKSpace>::
41 TangencyComputer( Clone<KSpace> K )
42  : myK( K ), myDConv( myK )
43 {
44  setUp();
45 }
46 
47 //-----------------------------------------------------------------------------
48 template < typename TKSpace >
49 template < typename PointIterator >
50 void
51 DGtal::TangencyComputer<TKSpace>::
52 init( PointIterator itB, PointIterator itE )
53 {
54  myX = std::vector< Point >( itB, itE );
55  myCellCover =
56  myDConv.makeCellCover( myX.cbegin(), myX.cend(), 1, KSpace::dimension - 1 );
57  for ( Size i = 0; i < myX.size(); ++i )
58  myPt2Index[ myX[ i ] ] = i;
59 }
60 
61 //-----------------------------------------------------------------------------
62 template < typename TKSpace >
63 bool
64 DGtal::TangencyComputer<TKSpace>::
65 arePointsCotangent( const Point& a, const Point& b ) const
66 {
67  return myDConv.isFullySubconvex( a, b, myCellCover );
68 }
69 
70 //-----------------------------------------------------------------------------
71 template < typename TKSpace >
72 std::vector< typename DGtal::TangencyComputer<TKSpace>::Index >
73 DGtal::TangencyComputer<TKSpace>::
74 getCotangentPoints( const Point& a ) const
75 {
76  // Breadth-first traversal from a
77  std::vector< Index > R; // result
78  std::set < Index > V; // visited or in queue
79  std::queue < Index > Q; // queue for breadth-first traversal
80  ASSERT( myPt2Index.find( a ) != myPt2Index.cend() );
81  const auto idx_a = myPt2Index.find( a )->second;
82  Q.push ( idx_a );
83  V.insert( idx_a );
84  while ( ! Q.empty() )
85  {
86  const auto j = Q.front();
87  const auto p = myX[ j ];
88  Q.pop();
89  for ( auto && v : myN ) {
90  const Point q = p + v;
91  const auto it = myPt2Index.find( q );
92  if ( it == myPt2Index.cend() ) continue; // not in X
93  const auto next = it->second;
94  if ( V.count( next ) ) continue; // already visited
95  if ( arePointsCotangent( a, q ) )
96  {
97  R.push_back( next );
98  V.insert( next );
99  Q.push ( next );
100  }
101  }
102  }
103  return R;
104 }
105 
106 //-----------------------------------------------------------------------------
107 template < typename TKSpace >
108 std::vector< typename DGtal::TangencyComputer<TKSpace>::Index >
109 DGtal::TangencyComputer<TKSpace>::
110 getCotangentPoints( const Point& a,
111  const std::vector< bool > & to_avoid ) const
112 {
113  // Breadth-first traversal from a
114  std::vector< Index > R; // result
115  std::set < Index > V; // visited or in queue
116  std::queue < Index > Q; // queue for breadth-first traversal
117  ASSERT( myPt2Index.find( a ) != myPt2Index.cend() );
118  const auto idx_a = myPt2Index.find( a )->second;
119  Q.push ( idx_a );
120  V.insert( idx_a );
121  while ( ! Q.empty() )
122  {
123  const auto j = Q.front();
124  const auto p = myX[ j ];
125  const auto ap = p - a;
126  Q.pop();
127  for ( auto && v : myN ) {
128  if ( ap.dot( v ) < 0.0 ) continue;
129  const Point q = p + v;
130  const auto it = myPt2Index.find( q );
131  if ( it == myPt2Index.cend() ) continue; // not in X
132  const auto next = it->second;
133  if ( to_avoid[ next ] ) continue; // to avoid
134  if ( V.count( next ) ) continue; // already visited
135  if ( arePointsCotangent( a, q ) )
136  {
137  R.push_back( next );
138  V.insert( next );
139  Q.push ( next );
140  }
141  }
142  }
143  return R;
144 }
145 
146 //-----------------------------------------------------------------------------
147 template < typename TKSpace >
148 std::vector< typename DGtal::TangencyComputer<TKSpace>::Index >
149 DGtal::TangencyComputer<TKSpace>::ShortestPaths::
150 getCotangentPoints( Index idx_a ) const
151 {
152  bool use_secure = mySecure <= sqrt( KSpace::dimension );
153  // Breadth-first traversal from a
154  std::vector< Index > R; // result
155  std::set < Index > V; // visited or in queue
156  std::queue < Index > Q; // queue for breadth-first traversal
157  const auto a = point( idx_a );
158  Q.push ( idx_a );
159  V.insert( idx_a );
160  while ( ! Q.empty() )
161  {
162  const auto j = Q.front();
163  const auto p = point( j );
164  const auto ap = p - a;
165  Q.pop();
166  for ( int i = 0; i < myTgcyComputer->myN.size(); i++ ) {
167  const auto & v = myTgcyComputer->myN[ i ];
168  if ( ap.dot( v ) < 0.0 ) continue; // going backward
169  const Point q = p + v;
170  const auto it = myTgcyComputer->myPt2Index.find( q );
171  if ( it == myTgcyComputer->myPt2Index.cend() ) continue; // not in X
172  const auto next = it->second;
173  if ( myVisited[ next ] ) continue; // to avoid
174  if ( V.count( next ) ) continue; // already visited
175  const auto d_a = myDistance[ idx_a ] + ( q - a ).norm();
176  if ( d_a >= ( myDistance[ next ]
177  + ( use_secure ? mySecure : myTgcyComputer->myDN[ i ] ) ) )
178  continue; // only if distance is better.
179  if ( myTgcyComputer->arePointsCotangent( a, q ) )
180  {
181  R.push_back( next );
182  V.insert( next );
183  Q.push ( next );
184  }
185  }
186  }
187  return R;
188 }
189 
190 //-----------------------------------------------------------------------------
191 template < typename TKSpace >
192 typename DGtal::TangencyComputer<TKSpace>::ShortestPaths
193 DGtal::TangencyComputer<TKSpace>::
194 makeShortestPaths( double secure ) const
195 {
196  return ShortestPaths( *this, secure );
197 }
198 
199 
200 //-----------------------------------------------------------------------------
201 template < typename TKSpace >
202 std::vector< typename DGtal::TangencyComputer<TKSpace>::Path >
203 DGtal::TangencyComputer<TKSpace>::
204 shortestPaths( const std::vector< Index >& sources,
205  const std::vector< Index >& targets,
206  double secure, bool verbose ) const
207 {
208  auto SP = makeShortestPaths( secure );
209  SP.init( targets.cbegin(), targets.cend() );
210  std::vector< Path > paths( sources.size() );
211  while ( ! SP.finished() )
212  {
213  auto n = SP.current();
214  SP.expand();
215  if ( verbose )
216  trace.info() << "Point " << point( std::get<0>( n ) )
217  << " at distance " << std::get<2>( n ) << std::endl;
218  }
219  for ( auto i = 0; i < sources.size(); i++ )
220  paths[ i ] = SP.pathToSource( sources[ i ] );
221  return paths;
222 }
223 
224 //-----------------------------------------------------------------------------
225 template < typename TKSpace >
226 typename DGtal::TangencyComputer<TKSpace>::Path
227 DGtal::TangencyComputer<TKSpace>::
228 shortestPath( Index source, Index target,
229  double secure, bool verbose ) const
230 {
231  auto SP0 = makeShortestPaths( secure );
232  auto SP1 = makeShortestPaths( secure );
233  SP0.init( source );
234  SP1.init( target );
235  Path Q;
236  while ( ! SP0.finished() && ! SP1.finished() )
237  {
238  auto n0 = SP0.current();
239  auto n1 = SP1.current();
240  auto p0 = std::get<0>( n0 );
241  auto p1 = std::get<0>( n1 );
242  SP0.expand();
243  SP1.expand();
244  if ( SP0.isVisited( p1 ) )
245  {
246  auto c0 = SP0.pathToSource( p1 );
247  auto c1 = SP1.pathToSource( p1 );
248  std::copy(c0.rbegin(), c0.rend(), std::back_inserter(Q));
249  Q.pop_back();
250  std::copy(c1.begin(), c1.end(), std::back_inserter(Q));
251  break;
252  }
253  if ( verbose )
254  {
255  double last_distance = std::get<2>( n0 ) + std::get<2>( n1 );
256  trace.info() << p0 << " " << p1 << " last_d=" << last_distance << std::endl;
257  }
258  }
259  return Q;
260 }
261 
262 //-----------------------------------------------------------------------------
263 template <typename TKSpace>
264 void
265 DGtal::TangencyComputer<TKSpace>::
266 setUp()
267 {
268  myN.clear();
269  myDN.clear();
270  const Point zero = Point::zero;
271  Domain neighborhood( Point::diagonal( -1 ), Point::diagonal( 1 ) );
272  for ( auto&& v : neighborhood )
273  {
274  if ( v != zero )
275  {
276  myN .push_back( v );
277  myDN.push_back( v.norm() );
278  }
279  }
280 }
281 
282 ///////////////////////////////////////////////////////////////////////////////
283 // class TangencyComputer::Shortestpaths
284 ///////////////////////////////////////////////////////////////////////////////
285 
286 //-----------------------------------------------------------------------------
287 template <typename TKSpace>
288 void
289 DGtal::TangencyComputer<TKSpace>::ShortestPaths::
290 expand()
291 {
292  ASSERT( ! finished() );
293  auto elem = myQ.top();
294  myQ.pop();
295  propagate( std::get<0>( elem ) );
296  Index current;
297  double d;
298  int nb = 0;
299  while ( ! finished() )
300  {
301  elem = myQ.top();
302  nb += 1;
303  current = std::get<0>( elem );
304  d = std::get<2>( elem );
305  ASSERT( ( ! ( myVisited[ current ] && ( d < myDistance[ current ] ) ) )
306  && "Already visited node and smaller distance" );
307  if ( ! myVisited[ current ] ) break;
308  myQ.pop();
309  }
310  if ( ! finished() )
311  {
312  myAncestor[ current ] = std::get<1>( elem );
313  myDistance[ current ] = d;
314  myVisited [ current ] = true;
315  }
316 }
317 
318 //-----------------------------------------------------------------------------
319 template <typename TKSpace>
320 void
321 DGtal::TangencyComputer<TKSpace>::ShortestPaths::
322 propagate( Index current )
323 {
324  auto eucl_d = [] ( const Point& p, const Point& q )
325  { return ( p - q ).norm(); };
326 
327  if ( ! myVisited[ current ] )
328  trace.warning() << "Propagate from unvisited node " << current << std::endl;
329  const Point q = myTgcyComputer->point( current );
330  std::vector< Index > N = getCotangentPoints( current );
331  for ( auto next : N )
332  {
333  if ( ! myVisited[ next ] )
334  {
335  const Point p = myTgcyComputer->point( next );
336  double next_d = myDistance[ current ] + eucl_d( q, p );
337  if ( next_d < myDistance[ next ] )
338  {
339  myDistance[ next ] = next_d;
340  myQ.push( std::make_tuple( next, current, next_d ) );
341  }
342  }
343  }
344 }
345 
346 
347 ///////////////////////////////////////////////////////////////////////////////
348 // Implementation of inline functions //
349 
350 //-----------------------------------------------------------------------------
351 template <typename TKSpace>
352 inline
353 std::ostream&
354 DGtal::operator<< ( std::ostream & out,
355  const TangencyComputer<TKSpace> & object )
356 {
357  object.selfDisplay( out );
358  return out;
359 }
360 
361 // //
362 ///////////////////////////////////////////////////////////////////////////////