iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
PolygonImpl.H
Go to the documentation of this file.
1 /*! @file
2  @author Randolph Voorhies
3  @copyright GNU Public License (GPL v3)
4  @section License
5  @verbatim
6  // ////////////////////////////////////////////////////////////////////////
7  // The iLab Neuromorphic Robotics Toolkit (NRT) //
8  // Copyright 2010-2012 by the University of Southern California (USC) //
9  // and the iLab at USC. //
10  // //
11  // iLab - University of Southern California //
12  // Hedco Neurociences Building, Room HNB-10 //
13  // Los Angeles, Ca 90089-2520 - USA //
14  // //
15  // See http://ilab.usc.edu for information about this project. //
16  // ////////////////////////////////////////////////////////////////////////
17  // This file is part of The iLab Neuromorphic Robotics Toolkit. //
18  // //
19  // The iLab Neuromorphic Robotics Toolkit is free software: you can //
20  // redistribute it and/or modify it under the terms of the GNU General //
21  // Public License as published by the Free Software Foundation, either //
22  // version 3 of the License, or (at your option) any later version. //
23  // //
24  // The iLab Neuromorphic Robotics Toolkit is distributed in the hope //
25  // that it will be useful, but WITHOUT ANY WARRANTY; without even the //
26  // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR //
27  // PURPOSE. See the GNU General Public License for more details. //
28  // //
29  // You should have received a copy of the GNU General Public License //
30  // along with The iLab Neuromorphic Robotics Toolkit. If not, see //
31  // <http://www.gnu.org/licenses/>. //
32  // ////////////////////////////////////////////////////////////////////////
33  @endverbatim */
34 
35 
36 // ######################################################################
37 template <class T> inline
39 { }
40 
41 // ######################################################################
42 template <class T> inline
44 { itsVertices = other.itsVertices; }
45 
46 // ######################################################################
47 template <class T> inline
49  itsVertices(std::move(other.itsVertices))
50 { }
51 
52 // ######################################################################
53 template <class T> inline
54 nrt::Polygon<T>::Polygon(std::vector<nrt::Point2D<T> > const& vertices) :
55  itsVertices(vertices)
56 { }
57 
58 // ######################################################################
59 template <class T> template <class U> inline
60 nrt::Polygon<T>::Polygon(std::vector<nrt::Point2D<U> > const & vertices)
61 {
62  for (nrt::Point2D<U> const & p : vertices) itsVertices.push_back(nrt::Point2D<T>(p));
63 }
64 
65 // ######################################################################
66 template <class T> template <class U> inline
68 {
69  for (nrt::Point2D<U> const & p : other.vertices()) itsVertices.push_back(nrt::Point2D<T>(p));
70 }
71 
72 // ######################################################################
73 template<class T> inline
75 {
76  itsVertices = other.itsVertices;
77  return *this;
78 }
79 
80 // ######################################################################
81 template <class T> inline
82 nrt::Polygon<T>::Polygon(std::initializer_list<nrt::Point2D<T> > list)
83 { for (Point2D<T> const & p : list) itsVertices.push_back(p); }
84 
85 // ######################################################################
86 template <class T> inline
87 std::vector<nrt::Point2D<T> > const & nrt::Polygon<T>::vertices() const
88 { return itsVertices; }
89 
90 // ######################################################################
91 template <class T> inline
92 size_t nrt::Polygon<T>::size() const
93 { return itsVertices.size(); }
94 
95 // ######################################################################
96 template <class T> inline
98 { return itsVertices[index]; }
99 
100 // ######################################################################
101 template <class T> inline
102 nrt::Point2D<T> const & nrt::Polygon<T>::vertex(size_t index) const
103 { return itsVertices[index]; }
104 
105 // ######################################################################
106 template <class T> inline
107 typename std::vector<nrt::Point2D<T> >::iterator nrt::Polygon<T>::begin()
108 { return itsVertices.begin(); }
109 
110 // ######################################################################
111 template <class T> inline
112 typename std::vector<nrt::Point2D<T> >::iterator nrt::Polygon<T>::end()
113 { return itsVertices.end(); }
114 
115 // ######################################################################
116 template <class T> inline
117 typename std::vector<nrt::Point2D<T> >::const_iterator nrt::Polygon<T>::begin() const
118 { return itsVertices.begin(); }
119 
120 // ######################################################################
121 template <class T> inline
122 typename std::vector<nrt::Point2D<T> >::const_iterator nrt::Polygon<T>::end() const
123 { return itsVertices.end(); }
124 
125 // ######################################################################
126 template <class T> template <typename promo> inline
128 {
129  typedef typename nrt::promote<T, promo>::type TP;
130  nrt::Point2D<TP> center(TP(0), TP(0));
131  for (nrt::Point2D<T> const & p : itsVertices) center += nrt::Point2D<TP>(p);
132  center /= double(itsVertices.size());
133  return center;
134 }
135 
136 // ######################################################################
137 template <class T> template <typename promo> inline
139 {
140  typedef typename nrt::promote<T, promo>::type TP;
141 
142  // formula adapted from wikipedia
143  nrt::Point2D<TP> centroid(TP(0), TP(0));
144  size_t const ps = itsVertices.size();
145  double a = 0.0; // area
146  for (size_t i = 0, j = ps-1; i < ps; j = i++)
147  {
148  double const crossterm = itsVertices[j].x()*itsVertices[i].y() - itsVertices[j].y()*itsVertices[i].x();
149  a += crossterm / 2;
150  centroid.x() += (itsVertices[j].x() + itsVertices[i].x()) * crossterm;
151  centroid.y() += (itsVertices[j].y() + itsVertices[i].y()) * crossterm;
152  }
153  centroid /= (6.0 * a); // unsigned area
154  return centroid;
155 }
156 
157 // ######################################################################
158 template <class T>
159 double nrt::Polygon<T>::area(const bool getsigned) const
160 {
161  double area = 0.0;
162  uint const ps = itsVertices.size();
163  for (uint i = 0, j = ps-1; i < ps; j = i++)
164  area += itsVertices[i].x()*itsVertices[j].y() - itsVertices[i].y()*itsVertices[j].x();
165  if (getsigned) return (0.5 * area);
166  else return fabs(0.5 * area);
167 }
168 
169 // ######################################################################
170 // Original algo by Randolph Franklin,
171 // http://astronomy.swin.edu.au/~pbourke/geometry/insidepoly/
172 template <class T> template <class U>
173 bool nrt::Polygon<T>::contains(Point2D<U> const & point) const
174 {
175  typedef typename nrt::promote<T,U>::type TP;
176  TP const px = point.x(); TP const py = point.y();
177 
178  if (itsVertices.size() == 3)
179  {
180  TP const ax = itsVertices[2].x() - itsVertices[1].x(); TP const ay = itsVertices[2].y() - itsVertices[1].y();
181  TP const bx = itsVertices[0].x() - itsVertices[2].x(); TP const by = itsVertices[0].y() - itsVertices[2].y();
182  TP const cx = itsVertices[1].x() - itsVertices[0].x(); TP const cy = itsVertices[1].y() - itsVertices[0].y();
183  TP const apx = px - itsVertices[0].x(); TP const apy = py - itsVertices[0].y();
184  TP const bpx = px - itsVertices[1].x(); TP const bpy = py - itsVertices[1].y();
185  TP const cpx = px - itsVertices[2].x(); TP const cpy = py - itsVertices[2].y();
186 
187  TP const aCROSSbp = ax*bpy - ay*bpx;
188  TP const cCROSSap = cx*apy - cy*apx;
189  TP const bCROSScp = bx*cpy - by*cpx;
190 
191  return ((aCROSSbp >= 0) && (bCROSScp >= 0) && (cCROSSap >= 0));
192  }
193  else
194  {
195  bool c = false; const uint ps = itsVertices.size();
196  for (uint i = 0, j = ps-1; i < ps; j = i++)
197  if ((((itsVertices[i].y() <= py) && (py < itsVertices[j].y())) ||
198  ((itsVertices[j].y() <= py) && (py < itsVertices[i].y()))) &&
199  (px < (itsVertices[j].x() - itsVertices[i].x()) *
200  (py - itsVertices[i].y()) / (itsVertices[j].y() - itsVertices[i].y())
201  + itsVertices[i].x()))
202  c = !c;
203 
204  return c;
205  }
206 }
207 
208 // ######################################################################
209 template <class T>
211 { return nrt::Rectangle<T>::bounds(itsVertices); }
212 
213 // ######################################################################
214 namespace nrt_private_polygon
215 {
216  // Determine which side of line ab that point c falls on
217  template <class T>
218  bool determineSide(nrt::Point2D<T> const & a, nrt::Point2D<T> const & b, nrt::Point2D<T> const & c)
219  {
220  typedef typename nrt::promote<T, float>::type TF;
221  TF const val = (c.x()-b.x())*(b.y()-a.y()) - (c.y()-b.y())*(b.x()-a.x());
222  return (val > 0.0F);
223  }
224 }
225 
226 // ######################################################################
227 template <class T>
229 {
230  std::vector<nrt::Point2D<T> > const oldV = itsVertices;
231  assert(oldV.size() >= 3);
232  std::deque<nrt::Point2D<T> > dq;
233 
234  if(nrt_private_polygon::determineSide(oldV[0],oldV[1],oldV[2]))
235  {
236  dq.push_back(oldV[0]);
237  dq.push_back(oldV[1]);
238  }
239  else
240  {
241  dq.push_back(oldV[1]);
242  dq.push_back(oldV[0]);
243  }
244  dq.push_back(oldV[2]);
245  dq.push_front(oldV[2]);
246  size_t ind=3;
247  bool complete = false;
248  for(ind=3;ind<oldV.size();ind++)
249  {
250  while(nrt_private_polygon::determineSide(dq[dq.size()-2],dq[dq.size()-1],oldV[ind]) &&
251  nrt_private_polygon::determineSide(oldV[ind],dq[0],dq[1]))
252  {
253  ind++;
254  if(ind>oldV.size()-1)
255  {
256  complete=true;
257  break;
258  }
259  }
260  if(complete)
261  break;
262  while(!nrt_private_polygon::determineSide(dq[dq.size()-2],dq[dq.size()-1],oldV[ind]) && dq.size()>0)
263  dq.pop_back();
264  dq.push_back(oldV[ind]);
265  while(!nrt_private_polygon::determineSide(oldV[ind],dq[0],dq[1]) && dq.size()>0)
266  dq.pop_front();
267  dq.push_front(oldV[ind]);
268  }
269  // Now get rid of extra vertex.
270  // NRT storage of a polygon assumes closure, ie that the last vertex and the first vertex are connected
271  dq.pop_front();
272  std::vector<nrt::Point2D<T> > newV;
273  // Copy to a vector
274  std::copy(dq.begin(),dq.end(),std::back_inserter(newV));
275  return nrt::Polygon<T>(newV);
276 }
277 
278 namespace nrt
279 {
280  namespace convexhulldetails
281  {
282  template <class T>
283  double crossproduct(Point2D<T> const& p1, Point2D<T> const& p2)
284  { return p1.x()*p2.y()-p1.y()*p2.x(); }
285 
286  template <class T>
287  double dotproduct(Point2D<T> const& p1, Point2D<T> const& p2)
288  { return p1.x()*p2.x()+p1.x()*p2.y(); }
289 
290  }
291 }
292 
293 // Copyright 2001, softSurfer (www.softsurfer.com)
294 // This code may be freely used and modified for any purpose
295 // providing that this copyright notice is included with it.
296 // SoftSurfer makes no warranty for this code, and cannot be held
297 // liable for any real or imagined damage resulting from its use.
298 // Users of this code must verify correctness for their application.
299 namespace nrt
300 {
301  namespace convexhullhelpers
302  {
303  // isLeft(): tests if a point is Left|On|Right of an infinite line.
304  // Input: three points P0, P1, and P2
305  // Return: >0 for P2 left of the line through P0 and P1
306  // =0 for P2 on the line
307  // <0 for P2 right of the line
308  // See: the January 2001 Algorithm on Area of Triangles
309  template <class T>
310  float isLeft( nrt::Point2D<T> const& P0, nrt::Point2D<T> const& P1, nrt::Point2D<T> const& P2 )
311  { return (P1.x() - P0.x())*(P2.y() - P0.y()) - (P2.x() - P0.x())*(P1.y() - P0.y()); }
312  }
313 }
314 
315 // Copyright 2001, softSurfer (www.softsurfer.com)
316 // This code may be freely used and modified for any purpose
317 // providing that this copyright notice is included with it.
318 // SoftSurfer makes no warranty for this code, and cannot be held
319 // liable for any real or imagined damage resulting from its use.
320 // Users of this code must verify correctness for their application.
321 template <class T>
323 {
324  if(points.size() <= 3) return nrt::Polygon<T>(points);
325 
326  std::vector<nrt::Point2D<T>> H(points.size());
327  std::vector<nrt::Point2D<T>> P = points;
328  std::sort(P.begin(), P.end(),
330  {
331  if(p1.x() == p2.x())
332  { return bool(p1.y() < p2.y()); }
333  else
334  { return bool(p1.x() < p2.x()); }
335  });
336 
337  int n = P.size();
338  // the output array H[] will be used as the stack
339  int bot=0, top=(-1); // indices for bottom and top of the stack
340  int i; // array scan index
341 
342  // Get the indices of points with min x-coord and min|max y-coord
343  int minmin = 0, minmax;
344  float xmin = P[0].x();
345  for (i=1; i<n; i++)
346  if (P[i].x() != xmin) break;
347  minmax = i-1;
348  if (minmax == n-1) { // degenerate case: all x-coords == xmin
349  H[++top] = P[minmin];
350  if (P[minmax].y() != P[minmin].y()) // a nontrivial segment
351  H[++top] = P[minmax];
352  H[++top] = P[minmin]; // add polygon endpoint
353  H.resize(top);
354  return nrt::Polygon<T>(H);
355  }
356 
357  // Get the indices of points with max x-coord and min|max y-coord
358  int maxmin, maxmax = n-1;
359  float xmax = P[n-1].x();
360  for (i=n-2; i>=0; i--)
361  if (P[i].x() != xmax) break;
362  maxmin = i+1;
363 
364  // Compute the lower hull on the stack H
365  H[++top] = P[minmin]; // push minmin point onto stack
366  i = minmax;
367  while (++i <= maxmin)
368  {
369  // the lower line joins P[minmin] with P[maxmin]
370  if (nrt::convexhullhelpers::isLeft( P[minmin], P[maxmin], P[i]) >= 0 && i < maxmin)
371  continue; // ignore P[i] above or on the lower line
372 
373  while (top > 0) // there are at least 2 points on the stack
374  {
375  // test if P[i] is left of the line at the stack top
376  if (nrt::convexhullhelpers::isLeft( H[top-1], H[top], P[i]) > 0)
377  break; // P[i] is a new hull vertex
378  else
379  top--; // pop top point off stack
380  }
381  H[++top] = P[i]; // push P[i] onto stack
382  }
383 
384  // Next, compute the upper hull on the stack H above the bottom hull
385  if (maxmax != maxmin) // if distinct xmax points
386  H[++top] = P[maxmax]; // push maxmax point onto stack
387  bot = top; // the bottom point of the upper hull stack
388  i = maxmin;
389  while (--i >= minmax)
390  {
391  // the upper line joins P[maxmax] with P[minmax]
392  if (nrt::convexhullhelpers::isLeft( P[maxmax], P[minmax], P[i]) >= 0 && i > minmax)
393  continue; // ignore P[i] below or on the upper line
394 
395  while (top > bot) // at least 2 points on the upper stack
396  {
397  // test if P[i] is left of the line at the stack top
398  if (nrt::convexhullhelpers::isLeft( H[top-1], H[top], P[i]) > 0)
399  break; // P[i] is a new hull vertex
400  else
401  top--; // pop top point off stack
402  }
403  H[++top] = P[i]; // push P[i] onto stack
404  }
405  if (minmax != minmin)
406  H[++top] = P[minmin]; // push joining endpoint onto stack
407 
408  H.resize(top);
409  return nrt::Polygon<T>(H);
410 }
411 
412 namespace nrt
413 {
414  namespace approximateconvexhullhelpers
415  {
416  const int NONE = -1;
417 
418  typedef struct range_bin Bin;
419  struct range_bin {
420  int min; // index of min point P[] in bin (>=0 or NONE)
421  int max; // index of max point P[] in bin (>=0 or NONE)
422  };
423 
424  inline double isLeft(nrt::Point2D<double> P0, nrt::Point2D<double> P1, nrt::Point2D<double> P2)
425  { return (P1.x() - P0.x())*(P2.y() - P0.y()) - (P2.x() - P0.x())*(P1.y() - P0.y()); }
426 
427  nrt::Polygon<double> computeHull(std::vector<nrt::Point2D<double>> const& P, int accuracy);
428  }
429 }
430 
431 // ######################################################################
432 template <class T>
434 {
435  std::vector<nrt::Point2D<double>> floatPoints;
436  for(nrt::Point2D<T> const & p : points)
437  floatPoints.push_back(nrt::Point2D<double>(p));
438  return nrt::Polygon<T>(nrt::approximateconvexhullhelpers::computeHull(floatPoints, accuracy));
439 }
440 
441 // ######################################################################
442 // Free function implementations
443 // ######################################################################
444 template <class T>
445 inline std::ostream& nrt::operator<< (std::ostream &out, nrt::Polygon<T> const& poly)
446 {
447  out << "{ ";
448  for(nrt::Point2D<T> const & point : poly.vertices()) out << "(" << point << ")";
449  out << " }";
450  return out;
451 }
452 
453 // ######################################################################
454 template <class T> inline
455 nrt::Polygon<T> nrt::scale(nrt::Polygon<T> const & src, double const factor)
456 {
457  nrt::Point2D<T> const c = nrt::Point2D<T>(src.template centroid<float>());
458 
459  nrt::Polygon<T> ret(src);
460  for (nrt::Point2D<T> & p : ret) p = c + (p - c) * factor;
461  return ret;
462 }
463 
464 // ######################################################################
465 template <class T> inline
466 nrt::Polygon<T> nrt::rotate(nrt::Polygon<T> const & src, double const angle)
467 {
468  nrt::Point2D<T> const c = nrt::Point2D<T>(src.template centroid<float>());
469  return nrt::rotateAbout(src, c, angle);
470 }
471 
472 // ######################################################################
473 template <class T> inline
474 nrt::Polygon<T> nrt::rotateAbout(nrt::Polygon<T> const & src, nrt::Point2D<T> const & point, double const angle)
475 {
476  nrt::Polygon<T> ret(src);
477  for (nrt::Point2D<T> & p : ret) p = nrt::rotateAbout(p, point, angle);
478  return ret;
479 }
480 
481 // ######################################################################
482 template <class T> inline
483 bool nrt::operator==(nrt::Polygon<T> const & lhs, nrt::Polygon<T> const & rhs)
484 { return std::equal(lhs.begin(), lhs.end(), rhs.begin()); }
485 
486 // ######################################################################
487 template <class T> inline
488 bool nrt::operator!=(nrt::Polygon<T> const & lhs, nrt::Polygon<T> const & rhs)
489 { return ! std::equal(lhs.begin(), lhs.end(), rhs.begin()); }
490 
491 // ######################################################################
492 template <class T> inline
494 { nrt::Polygon<T> ret(lhs); ret += rhs; return ret; }
495 
496 // ######################################################################
497 template <class T> inline
499 { nrt::Polygon<T> ret(rhs); ret += lhs; return ret; }
500 
501 // ######################################################################
502 template <class T> inline
504 { nrt::Polygon<T> ret(lhs); ret -= lhs; return ret; }
505 
506 // ######################################################################
507 template <class T> inline
509 {
510  nrt::Polygon<T> ret(rhs);
511  for (nrt::Point2D<T> & p : ret) p = lhs - p;
512  return ret;
513 }
514 
515 // ######################################################################
516 template <class T> inline
517 nrt::Polygon<T> nrt::operator*(nrt::Polygon<T> const & lhs, double const rhs)
518 { nrt::Polygon<T> ret(lhs); ret *= rhs; return ret; }
519 
520 // ######################################################################
521 template <class T> inline
522 nrt::Polygon<T> nrt::operator*(double const lhs, nrt::Polygon<T> const & rhs)
523 { nrt::Polygon<T> ret(rhs); ret *= lhs; return ret; }
524 
525 // ######################################################################
526 template <class T> inline
527 nrt::Polygon<T> nrt::operator/(nrt::Polygon<T> const & lhs, double const rhs)
528 {
529  nrt::Polygon<T> ret(lhs);
530  for (nrt::Point2D<T> & p : ret) p /= rhs;
531  return ret;
532 }
533 
534 // ######################################################################
535 template <class T> inline
536 nrt::Polygon<T> & nrt::operator*=(nrt::Polygon<T> & lhs, double const rhs)
537 {
538  for (nrt::Point2D<T> & p : lhs) p *= rhs;
539  return lhs;
540 }
541 
542 // ######################################################################
543 template <class T> inline
544 nrt::Polygon<T> & nrt::operator/=(nrt::Polygon<T> & lhs, double const rhs)
545 {
546  for (nrt::Point2D<T> & p : lhs) p /= rhs;
547  return lhs;
548 }
549 
550 // ######################################################################
551 template <class T> inline
553 {
554  for (nrt::Point2D<T> & p : lhs) p += rhs;
555  return lhs;
556 }
557 
558 // ######################################################################
559 template <class T> inline
561 {
562  for (nrt::Point2D<T> & p : lhs) p -= rhs;
563  return lhs;
564 }
565