DGtal  1.4.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 ), myLatticeCellCover( false )
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, bool use_lattice_cell_cover )
53 {
54  myX = std::vector< Point >( itB, itE );
55  myUseLatticeCellCover = use_lattice_cell_cover;
56  if ( use_lattice_cell_cover )
57  myLatticeCellCover = LatticeCellCover( myX.cbegin(), myX.cend() ).starOfPoints();
58  else
59  myCellCover =
60  myDConv.makeCellCover( myX.cbegin(), myX.cend(), 1, KSpace::dimension - 1 );
61  for ( Size i = 0; i < myX.size(); ++i )
62  myPt2Index[ myX[ i ] ] = i;
63 }
64 
65 //-----------------------------------------------------------------------------
66 template < typename TKSpace >
67 bool
68 DGtal::TangencyComputer<TKSpace>::
69 arePointsCotangent( const Point& a, const Point& b ) const
70 {
71  return myUseLatticeCellCover
72  ? myDConv.isFullySubconvex( a, b, myLatticeCellCover )
73  : myDConv.isFullySubconvex( a, b, myCellCover );
74 }
75 
76 //-----------------------------------------------------------------------------
77 template < typename TKSpace >
78 bool
79 DGtal::TangencyComputer<TKSpace>::
80 arePointsCotangent( const Point& a, const Point& b, const Point& c ) const
81 {
82  std::vector< Point > Z { a, b, c };
83  const auto P = myDConv.makePolytope( Z );
84  return myUseLatticeCellCover
85  ? myDConv.isFullySubconvex( P, myLatticeCellCover )
86  : myDConv.isFullySubconvex( P, myCellCover );
87 }
88 
89 //-----------------------------------------------------------------------------
90 template < typename TKSpace >
91 std::vector< typename DGtal::TangencyComputer<TKSpace>::Index >
92 DGtal::TangencyComputer<TKSpace>::
93 getCotangentPoints( const Point& a ) const
94 {
95  // Breadth-first traversal from a
96  std::vector< Index > R; // result
97  std::set < Index > V; // visited or in queue
98  std::queue < Index > Q; // queue for breadth-first traversal
99  ASSERT( myPt2Index.find( a ) != myPt2Index.cend() );
100  const auto idx_a = myPt2Index.find( a )->second;
101  Q.push ( idx_a );
102  V.insert( idx_a );
103  while ( ! Q.empty() )
104  {
105  const auto j = Q.front();
106  const auto p = myX[ j ];
107  Q.pop();
108  for ( auto && v : myN ) {
109  const Point q = p + v;
110  const auto it = myPt2Index.find( q );
111  if ( it == myPt2Index.cend() ) continue; // not in X
112  const auto next = it->second;
113  if ( V.count( next ) ) continue; // already visited
114  if ( arePointsCotangent( a, q ) )
115  {
116  R.push_back( next );
117  V.insert( next );
118  Q.push ( next );
119  }
120  }
121  }
122  return R;
123 }
124 
125 //-----------------------------------------------------------------------------
126 template < typename TKSpace >
127 std::vector< typename DGtal::TangencyComputer<TKSpace>::Index >
128 DGtal::TangencyComputer<TKSpace>::
129 getCotangentPoints( const Point& a,
130  const std::vector< bool > & to_avoid ) const
131 {
132  // Breadth-first traversal from a
133  std::vector< Index > R; // result
134  std::set < Index > V; // visited or in queue
135  std::queue < Index > Q; // queue for breadth-first traversal
136  ASSERT( myPt2Index.find( a ) != myPt2Index.cend() );
137  const auto idx_a = myPt2Index.find( a )->second;
138  Q.push ( idx_a );
139  V.insert( idx_a );
140  while ( ! Q.empty() )
141  {
142  const auto j = Q.front();
143  const auto p = myX[ j ];
144  const auto ap = p - a;
145  Q.pop();
146  for ( auto && v : myN ) {
147  if ( ap.dot( v ) < 0.0 ) continue;
148  const Point q = p + v;
149  const auto it = myPt2Index.find( q );
150  if ( it == myPt2Index.cend() ) continue; // not in X
151  const auto next = it->second;
152  if ( to_avoid[ next ] ) continue; // to avoid
153  if ( V.count( next ) ) continue; // already visited
154  if ( arePointsCotangent( a, q ) )
155  {
156  R.push_back( next );
157  V.insert( next );
158  Q.push ( next );
159  }
160  }
161  }
162  return R;
163 }
164 
165 //-----------------------------------------------------------------------------
166 template < typename TKSpace >
167 std::vector< typename DGtal::TangencyComputer<TKSpace>::Index >
168 DGtal::TangencyComputer<TKSpace>::ShortestPaths::
169 getCotangentPoints( Index idx_a ) const
170 {
171  bool use_secure = mySecure <= sqrt( KSpace::dimension );
172  // Breadth-first traversal from a
173  std::vector< Index > R; // result
174  std::set < Index > V; // visited or in queue
175  std::queue < Index > Q; // queue for breadth-first traversal
176  const auto a = point( idx_a );
177  Q.push ( idx_a );
178  V.insert( idx_a );
179  while ( ! Q.empty() )
180  {
181  const auto j = Q.front();
182  const auto p = point( j );
183  const auto ap = p - a;
184  Q.pop();
185  for ( size_t i = 0; i < myTgcyComputer->myN.size(); i++ ) {
186  const auto & v = myTgcyComputer->myN[ i ];
187  if ( ap.dot( v ) < 0.0 ) continue; // going backward
188  const Point q = p + v;
189  const auto it = myTgcyComputer->myPt2Index.find( q );
190  if ( it == myTgcyComputer->myPt2Index.cend() ) continue; // not in X
191  const auto next = it->second;
192  if ( myVisited[ next ] ) continue; // to avoid
193  if ( V.count( next ) ) continue; // already visited
194  const auto d_a = myDistance[ idx_a ] + ( q - a ).norm();
195  if ( d_a >= ( myDistance[ next ]
196  + ( use_secure ? mySecure : myTgcyComputer->myDN[ i ] ) ) )
197  continue; // only if distance is better.
198  if ( myTgcyComputer->arePointsCotangent( a, q ) )
199  {
200  R.push_back( next );
201  V.insert( next );
202  Q.push ( next );
203  }
204  }
205  }
206  return R;
207 }
208 
209 //-----------------------------------------------------------------------------
210 template < typename TKSpace >
211 typename DGtal::TangencyComputer<TKSpace>::ShortestPaths
212 DGtal::TangencyComputer<TKSpace>::
213 makeShortestPaths( double secure ) const
214 {
215  return ShortestPaths( *this, secure );
216 }
217 
218 
219 //-----------------------------------------------------------------------------
220 template < typename TKSpace >
221 std::vector< typename DGtal::TangencyComputer<TKSpace>::Path >
222 DGtal::TangencyComputer<TKSpace>::
223 shortestPaths( const std::vector< Index >& sources,
224  const std::vector< Index >& targets,
225  double secure, bool verbose ) const
226 {
227  auto SP = makeShortestPaths( secure );
228  SP.init( targets.cbegin(), targets.cend() );
229  std::vector< Path > paths( sources.size() );
230  while ( ! SP.finished() )
231  {
232  auto n = SP.current();
233  SP.expand();
234  if ( verbose )
235  trace.info() << "Point " << point( std::get<0>( n ) )
236  << " at distance " << std::get<2>( n ) << std::endl;
237  }
238  for ( auto i = 0; i < sources.size(); i++ )
239  paths[ i ] = SP.pathToSource( sources[ i ] );
240  return paths;
241 }
242 
243 //-----------------------------------------------------------------------------
244 template < typename TKSpace >
245 typename DGtal::TangencyComputer<TKSpace>::Path
246 DGtal::TangencyComputer<TKSpace>::
247 shortestPath( Index source, Index target,
248  double secure, bool verbose ) const
249 {
250  auto SP0 = makeShortestPaths( secure );
251  auto SP1 = makeShortestPaths( secure );
252  SP0.init( source );
253  SP1.init( target );
254  Path Q;
255  while ( ! SP0.finished() && ! SP1.finished() )
256  {
257  auto n0 = SP0.current();
258  auto n1 = SP1.current();
259  auto p0 = std::get<0>( n0 );
260  auto p1 = std::get<0>( n1 );
261  SP0.expand();
262  SP1.expand();
263  if ( SP0.isVisited( p1 ) )
264  {
265  auto c0 = SP0.pathToSource( p1 );
266  auto c1 = SP1.pathToSource( p1 );
267  std::copy(c0.rbegin(), c0.rend(), std::back_inserter(Q));
268  Q.pop_back();
269  std::copy(c1.begin(), c1.end(), std::back_inserter(Q));
270  break;
271  }
272  if ( verbose )
273  {
274  double last_distance = std::get<2>( n0 ) + std::get<2>( n1 );
275  trace.info() << p0 << " " << p1 << " last_d=" << last_distance << std::endl;
276  }
277  }
278  return Q;
279 }
280 
281 //-----------------------------------------------------------------------------
282 template <typename TKSpace>
283 void
284 DGtal::TangencyComputer<TKSpace>::
285 setUp()
286 {
287  myN.clear();
288  myDN.clear();
289  const Point zero = Point::zero;
290  Domain neighborhood( Point::diagonal( -1 ), Point::diagonal( 1 ) );
291  for ( auto&& v : neighborhood )
292  {
293  if ( v != zero )
294  {
295  myN .push_back( v );
296  myDN.push_back( v.norm() );
297  }
298  }
299 }
300 
301 ///////////////////////////////////////////////////////////////////////////////
302 // class TangencyComputer::Shortestpaths
303 ///////////////////////////////////////////////////////////////////////////////
304 
305 //-----------------------------------------------------------------------------
306 template <typename TKSpace>
307 void
308 DGtal::TangencyComputer<TKSpace>::ShortestPaths::
309 expand()
310 {
311  ASSERT( ! finished() );
312  auto elem = myQ.top();
313  myQ.pop();
314  propagate( std::get<0>( elem ) );
315  Index current;
316  double d;
317  while ( ! finished() )
318  {
319  elem = myQ.top();
320  current = std::get<0>( elem );
321  d = std::get<2>( elem );
322  ASSERT( ( ! ( myVisited[ current ] && ( d < myDistance[ current ] ) ) )
323  && "Already visited node and smaller distance" );
324  if ( ! myVisited[ current ] ) break;
325  myQ.pop();
326  }
327  if ( ! finished() )
328  {
329  myAncestor[ current ] = std::get<1>( elem );
330  myDistance[ current ] = d;
331  myVisited [ current ] = true;
332  }
333 }
334 
335 //-----------------------------------------------------------------------------
336 template <typename TKSpace>
337 void
338 DGtal::TangencyComputer<TKSpace>::ShortestPaths::
339 propagate( Index current )
340 {
341  auto eucl_d = [] ( const Point& p, const Point& q )
342  { return ( p - q ).norm(); };
343 
344  if ( ! myVisited[ current ] )
345  trace.warning() << "Propagate from unvisited node " << current << std::endl;
346  const Point q = myTgcyComputer->point( current );
347  std::vector< Index > N = getCotangentPoints( current );
348  for ( auto next : N )
349  {
350  if ( ! myVisited[ next ] )
351  {
352  const Point p = myTgcyComputer->point( next );
353  double next_d = myDistance[ current ] + eucl_d( q, p );
354  if ( next_d < myDistance[ next ] )
355  {
356  myDistance[ next ] = next_d;
357  myQ.push( std::make_tuple( next, current, next_d ) );
358  }
359  }
360  }
361 }
362 
363 
364 ///////////////////////////////////////////////////////////////////////////////
365 // Implementation of inline functions //
366 
367 //-----------------------------------------------------------------------------
368 template <typename TKSpace>
369 inline
370 std::ostream&
371 DGtal::operator<< ( std::ostream & out,
372  const TangencyComputer<TKSpace> & object )
373 {
374  object.selfDisplay( out );
375  return out;
376 }
377 
378 // //
379 ///////////////////////////////////////////////////////////////////////////////