DGtal  1.2.beta
PlaneProbingTetrahedronEstimator.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
19  * @author Jocelyn Meyron (\c jocelyn.meyron@liris.cnrs.fr )
20  * Laboratoire d'InfoRmatique en Image et Systemes d'information - LIRIS (CNRS, UMR 5205), CNRS, France
21  *
22  * @date 2020/12/04
23  *
24  * Implementation of inline methods defined in PlaneProbingTetrahedronEstimator.h
25  *
26  * This file is part of the DGtal library.
27  */
28 
29 
30 //////////////////////////////////////////////////////////////////////////////
31 #include <cassert>
32 #include <cstdlib>
33 #include <array>
34 #include <vector>
35 #include "DGtal/geometry/helpers/PlaneProbingEstimatorHelper.h"
36 #include "DGtal/geometry/surfaces/estimation/PlaneProbingHNeighborhood.h"
37 #include "DGtal/geometry/surfaces/estimation/PlaneProbingRNeighborhood.h"
38 #include "DGtal/geometry/surfaces/estimation/PlaneProbingR1Neighborhood.h"
39 //////////////////////////////////////////////////////////////////////////////
40 
41 ///////////////////////////////////////////////////////////////////////////////
42 // IMPLEMENTATION of inline methods.
43 ///////////////////////////////////////////////////////////////////////////////
44 
45 namespace DGtal
46 {
47  namespace detail
48  {
49  // Helper class to choose the PlaneProbingNeighborhood class at compile-time
50  // (used in the constructor of PlaneProbingTetrahedronEstimator)
51  template < typename TPredicate, DGtal::ProbingMode mode >
52  struct PlaneProbingNeighborhoodSelector
53  {
54  static DGtal::PlaneProbingNeighborhood<TPredicate>*
55  select(TPredicate const&,
56  typename DGtal::PlaneProbingNeighborhood<TPredicate>::Point const&,
57  typename DGtal::PlaneProbingNeighborhood<TPredicate>::Triangle const&);
58  };
59 
60  template < typename TPredicate >
61  struct PlaneProbingNeighborhoodSelector<TPredicate, DGtal::ProbingMode::H>
62  {
63  static DGtal::PlaneProbingNeighborhood<TPredicate>*
64  select(TPredicate const& aPredicate,
65  typename DGtal::PlaneProbingNeighborhood<TPredicate>::Point const& aQ,
66  typename DGtal::PlaneProbingNeighborhood<TPredicate>::Triangle const& aM)
67  {
68  return new DGtal::PlaneProbingHNeighborhood<TPredicate>(aPredicate, aQ, aM);
69  }
70  };
71 
72  template < typename TPredicate >
73  struct PlaneProbingNeighborhoodSelector<TPredicate, DGtal::ProbingMode::R>
74  {
75  static DGtal::PlaneProbingNeighborhood<TPredicate>*
76  select(TPredicate const& aPredicate,
77  typename DGtal::PlaneProbingNeighborhood<TPredicate>::Point const& aQ,
78  typename DGtal::PlaneProbingNeighborhood<TPredicate>::Triangle const& aM)
79  {
80  return new DGtal::PlaneProbingRNeighborhood<TPredicate>(aPredicate, aQ, aM);
81  }
82  };
83 
84  template < typename TPredicate >
85  struct PlaneProbingNeighborhoodSelector<TPredicate, DGtal::ProbingMode::R1>
86  {
87  static DGtal::PlaneProbingNeighborhood<TPredicate>*
88  select(TPredicate const& aPredicate,
89  typename DGtal::PlaneProbingNeighborhood<TPredicate>::Point const& aQ,
90  typename DGtal::PlaneProbingNeighborhood<TPredicate>::Triangle const& aM)
91  {
92  return new DGtal::PlaneProbingR1Neighborhood<TPredicate>(aPredicate, aQ, aM);
93  }
94  };
95 
96  } // namespace detail
97 } // namespace DGtal
98 
99 ///////////////////////////////////////////////////////////////////////////////
100 // ----------------------- Standard services ------------------------------
101 
102 // ------------------------------------------------------------------------
103 template < typename TPredicate, DGtal::ProbingMode mode >
104 inline
105 DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::
106 PlaneProbingTetrahedronEstimator (Point const& aPoint, Triangle const& aM, Predicate const& aPredicate)
107  : myM(aM), myPredicate(aPredicate), myS(aM[0] + aM[1] + aM[2]), myQ(aPoint + myS)
108 {
109  myNeighborhood = DGtal::detail::PlaneProbingNeighborhoodSelector<TPredicate, mode>::select(myPredicate, myQ, myM);
110 }
111 
112 // ------------------------------------------------------------------------
113 template < typename TPredicate, DGtal::ProbingMode mode >
114 inline
115 DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::
116 ~PlaneProbingTetrahedronEstimator ()
117 {
118  delete myNeighborhood;
119  myNeighborhood = nullptr;
120 }
121 
122 ///////////////////////////////////////////////////////////////////////////////
123 // ----------------------- Plane probing services ------------------------------
124 
125 // ------------------------------------------------------------------------
126 template < typename TPredicate, DGtal::ProbingMode mode >
127 inline
128 typename DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::Vector const&
129 DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::m (int aIndex) const
130 {
131  assert(aIndex == 0 || aIndex == 1 || aIndex == 2);
132  return myM[aIndex];
133 }
134 
135 // ------------------------------------------------------------------------
136 template < typename TPredicate, DGtal::ProbingMode mode >
137 inline
138 typename DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::Point const&
139 DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::q () const
140 {
141  return myQ;
142 }
143 
144 // ------------------------------------------------------------------------
145 template < typename TPredicate, DGtal::ProbingMode mode >
146 inline
147 typename DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::Point
148 DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::getOrigin () const
149 {
150  return myQ - (myM[0] + myM[1] + myM[2]);
151 }
152 
153 // ------------------------------------------------------------------------
154 template < typename TPredicate, DGtal::ProbingMode mode >
155 inline
156 typename DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::Triangle
157 DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::vertices () const
158 {
159  return { myQ - myM[0], myQ - myM[1], myQ - myM[2] };
160 }
161 
162 // ------------------------------------------------------------------------
163 template < typename TPredicate, DGtal::ProbingMode mode >
164 inline
165 std::pair<typename DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::Vector,
166  typename DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::Vector>
167 DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::getBasis () const
168 {
169  using DGtal::detail::squaredNorm;
170 
171  Vector u = myM[1] - myM[0],
172  v = myM[2] - myM[1],
173  w = myM[0] - myM[2];
174  ASSERT(w == -u - v);
175 
176  if (squaredNorm(u) < squaredNorm(v)) {
177  if (squaredNorm(u) < squaredNorm(w)) {
178  if (squaredNorm(-w) < squaredNorm(v)) {
179  return std::make_pair(u, -w);
180  } else {
181  return std::make_pair(u, v);
182  }
183  } else {
184  if (squaredNorm(-v) < squaredNorm(u)) {
185  return std::make_pair(w, -v);
186  } else {
187  return std::make_pair(w, u);
188  }
189  }
190  } else {
191  if (squaredNorm(v) < squaredNorm(w)) {
192  if (squaredNorm(-u) < squaredNorm(w)) {
193  return std::make_pair(v, -u);
194  } else {
195  return std::make_pair(v, w);
196  }
197  } else {
198  if (squaredNorm(-v) < squaredNorm(u)) {
199  return std::make_pair(w, -v);
200  } else {
201  return std::make_pair(w, u);
202  }
203  }
204  }
205 }
206 
207 // ------------------------------------------------------------------------
208 template < typename TPredicate, DGtal::ProbingMode mode >
209 inline
210 bool
211 DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::isReduced () const
212 {
213  auto basis = getBasis();
214  return DGtal::detail::isBasisReduced(basis.first, basis.second);
215 }
216 
217 // ------------------------------------------------------------------------
218 template < typename TPredicate, DGtal::ProbingMode mode >
219 inline
220 typename DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::Vector
221 DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::getNormal () const
222 {
223  auto basis = getBasis();
224  return basis.first.crossProduct(basis.second);
225 }
226 
227 // ------------------------------------------------------------------------
228 template < typename TPredicate, DGtal::ProbingMode mode >
229 inline
230 std::pair<bool, typename DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::UpdateOperation>
231 DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::advance (std::vector<PointOnProbingRay> const& aNeighbors)
232 {
233  myNeighborhood->setNeighbors(aNeighbors);
234  HexagonState state = hexagonState();
235 
236  if (state == HexagonState::Planar)
237  {
238  UpdateOperation op = myNeighborhood->closestCandidate();
239  update(op);
240  ASSERT(isValid());
241  return { true, op };
242  }
243 
244  return { false, {} };
245 }
246 
247 // ------------------------------------------------------------------------
248 template < typename TPredicate, DGtal::ProbingMode mode >
249 inline
250 std::pair<bool, typename DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::UpdateOperation>
251 DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::advance ()
252 {
253  return advance({});
254 }
255 
256 // ------------------------------------------------------------------------
257 template < typename TPredicate, DGtal::ProbingMode mode >
258 inline
259 typename DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::Quantity
260 DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::compute (std::vector<PointOnProbingRay> const& aNeighbors)
261 {
262  while (advance(aNeighbors).first) {}
263 
264  return getNormal();
265 }
266 
267 // ------------------------------------------------------------------------
268 template < typename TPredicate, DGtal::ProbingMode mode >
269 inline
270 typename DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::Quantity
271 DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::compute ()
272 {
273  return compute({});
274 }
275 
276 // ------------------------------------------------------------------------
277 template < typename TPredicate, DGtal::ProbingMode mode >
278 inline
279 typename DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::Neighborhood::HexagonState
280 DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::hexagonState () const
281 {
282  return myNeighborhood->hexagonState();
283 }
284 
285 // ------------------------------------------------------------------------
286 template < typename TPredicate, DGtal::ProbingMode mode >
287 inline
288 void
289 DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::translateQ (Vector const& aTranslation)
290 {
291  myQ += aTranslation;
292 }
293 
294 // ------------------------------------------------------------------------
295 template < typename TPredicate, DGtal::ProbingMode mode >
296 inline
297 void
298 DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::translateQ ()
299 {
300  UpdateOperation const& lastOp = myOperations[myOperations.size() - 1];
301  Point translation = lastOp.coeffs[1] * myM[lastOp.sigma[1]] + lastOp.coeffs[2] * myM[lastOp.sigma[2]];
302 
303  translateQ(translation);
304 }
305 
306 ///////////////////////////////////////////////////////////////////////////////
307 // ------------------------- Internals ------------------------------------
308 
309 // ------------------------------------------------------------------------
310 template < typename TPredicate, DGtal::ProbingMode mode >
311 inline
312 bool
313 DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::isProjectedInside (Triangle const& aTriangle) const
314 {
315  Triangle vec = { myQ - aTriangle[0], myQ - aTriangle[1], myQ - aTriangle[2] };
316 
317  Point s = myM[0] + myM[1] + myM[2];
318  std::array<bool, 3> res, res_not;
319  for (int i = 0; i < 3; ++i)
320  {
321  int im1 = (i - 1 + 3) % 3;
322  Point nk = vec[im1].crossProduct(vec[i]);
323 
324  res[i] = (nk.dot(-s) <= 0);
325  res_not[i] = !res[i];
326  }
327 
328  return (res[0] && res[1] && res[2]) || (res_not[0] && res_not[1] && res_not[2]);
329 }
330 
331 // ------------------------------------------------------------------------
332 template < typename TPredicate, DGtal::ProbingMode mode >
333 inline
334 void
335 DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::applyOperation (UpdateOperation const& aOp)
336 {
337  myM[aOp.sigma[0]] = aOp.coeffs[0] * myM[aOp.sigma[0]] + aOp.coeffs[1] * myM[aOp.sigma[1]] + aOp.coeffs[2] * myM[aOp.sigma[2]];
338 }
339 
340 // ------------------------------------------------------------------------
341 template < typename TPredicate, DGtal::ProbingMode mode >
342 inline
343 void
344 DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::update (UpdateOperation const& aOp)
345 {
346  applyOperation(aOp);
347  myOperations.push_back(aOp);
348 }
349 
350 ///////////////////////////////////////////////////////////////////////////////
351 // Interface - public :
352 
353 /**
354  * Writes/Displays the object on an output stream.
355  * @param out the output stream where the object is written.
356  */
357 template <typename TPredicate, DGtal::ProbingMode mode>
358 inline
359 void
360 DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::selfDisplay ( std::ostream & out ) const
361 {
362  out << "[PlaneProbingTetrahedronEstimator]";
363 }
364 
365 /**
366  * Checks the validity/consistency of the object.
367  * @return 'true' if the object is valid, 'false' otherwise.
368  */
369 template <typename TPredicate, DGtal::ProbingMode mode>
370 inline
371 bool
372 DGtal::PlaneProbingTetrahedronEstimator<TPredicate, mode>::isValid() const
373 {
374  Triangle v = vertices();
375  for (int i = 0; i < 3; ++i) {
376  if (! myPredicate(v[i])) {
377  return false;
378  }
379  }
380 
381  Point nest = getNormal();
382  for (int i = 0; i < 3; ++i) {
383  if (myM[i].dot(nest) != 1) {
384  return false;
385  }
386  }
387 
388  if (! isProjectedInside(v)) {
389  return false;
390  }
391 
392  return true;
393 }
394 
395 ///////////////////////////////////////////////////////////////////////////////
396 // Implementation of inline functions //
397 
398 template <typename TPredicate, DGtal::ProbingMode mode>
399 inline
400 std::ostream&
401 DGtal::operator<< ( std::ostream & out,
402  const PlaneProbingTetrahedronEstimator<TPredicate, mode> & object )
403 {
404  object.selfDisplay( out );
405  return out;
406 }
407 
408 // //
409 ///////////////////////////////////////////////////////////////////////////////
410 
411