iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
RotatedRectangleImpl.H
Go to the documentation of this file.
1 /*! @file
2  @author Unknown
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 template <class T>
36 nrt::RotatedRectangle<T>::RotatedRectangle(nrt::Point2D<T> const & center, Dims<T> const & dims, double const & angle) :
37  itsCenter(center), itsDims(dims), itsAngle(angle)
38 { }
39 
40 // ######################################################################
41 template <class T>
42  template<class U>
44  itsCenter(other.center()), itsDims(other.dims()), itsAngle(other.angle())
45 { }
46 
47 // ######################################################################
48 template <class T>
50 { return itsCenter; }
51 
52 // ######################################################################
53 template <class T>
55 { return itsDims; }
56 
57 // ######################################################################
58 template <class T>
60 { return itsAngle; }
61 
62 // ######################################################################
63 template <class T>
64 std::vector<nrt::Point2D<T>> nrt::RotatedRectangle<T>::vertices() const
65 {
66  T const x = itsDims.width() / 2.0;
67  T const y = itsDims.height() / 2.0;
68 
69  return {
70  nrt::rotateAbout(nrt::Point2D<T>(itsCenter + nrt::Point2D<T>( x, y)), itsCenter, itsAngle),
71  nrt::rotateAbout(nrt::Point2D<T>(itsCenter + nrt::Point2D<T>(-x, y)), itsCenter, itsAngle),
72  nrt::rotateAbout(nrt::Point2D<T>(itsCenter + nrt::Point2D<T>(-x, -y)), itsCenter, itsAngle),
73  nrt::rotateAbout(nrt::Point2D<T>(itsCenter + nrt::Point2D<T>( x, -y)), itsCenter, itsAngle)
74  };
75 }
76 
77 namespace nrt
78 {
79  namespace rotatedrectangledetails
80  {
81  double mod(double i, double N)
82  {
83  while(i >= N) i-= N;
84  while(i < 0) i += N;
85  return i;
86  }
87 
88  template<class T>
89  double DOTPR(int cur, int nxt, std::vector<nrt::Point2D<T>> const & vertices)
90  {
91  size_t const N = vertices.size();
92  Point2D<T> const c1 = vertices.at(mod( cur , N));
93  Point2D<T> const c2 = vertices.at(mod(cur+1 , N));
94  Point2D<T> const n1 = vertices.at(mod( nxt , N));
95  Point2D<T> const n2 = vertices.at(mod((nxt+1), N));
96  Point2D<T> const vc = c1 - c2;
97  Point2D<T> const vn = n1 - n2;
98  double const dotpr = vc.x()*vn.x()+vc.y()*vn.y();
99  return dotpr;
100  }
101 
102  template<class T>
103  double CROSSPR(int cur, int nxt, std::vector<nrt::Point2D<T>> const & vertices)
104  {
105  Point2D<T> const c1 = vertices.at(cur);
106  Point2D<T> const c2 = vertices.at(mod(cur+1, vertices.size()));
107  Point2D<T> const n1 = vertices.at(nxt);
108  Point2D<T> const n2 = vertices.at(mod(nxt+1, vertices.size()));
109  Point2D<T> const vc = c1 - c2;
110  Point2D<T> const vn = n1 - n2;
111  return vc.x()*vn.y()-vc.y()*vn.x();
112  }
113 
114  template<class T>
115  double ANGLE(int i, std::vector<nrt::Point2D<T>> const & v)
116  {
117  int const N = v.size();
118  nrt::Point2D<double> const v1(v.at(i) - v.at(mod(i-1,N)));
119  nrt::Point2D<double> const v2(v.at(mod(i+1,N)) - v.at(i));
120  return acos((v1.x()*v2.x()+v1.y()*v2.y())/(v1.magnitude()*v2.magnitude()));
121  }
122 
123  // Compute the cross product : AB x AC
124  template<class T>
125  double cross(nrt::Point2D<T> const & A, nrt::Point2D<T> const & B, nrt::Point2D<T> const & C)
126  {
127  Point2D<double> AB, AC;
128  AB[0] = B[0] - A[0];
129  AB[1] = B[1] - A[1];
130  AC[0] = C[0] - A[0];
131  AC[1] = C[1] - A[1];
132  double const cross = AB[0] * AC[1] - AB[1] * AC[0];
133  return cross;
134  }
135 
136  // Compute the distance from the segment AB to the point C
137  template<class T>
138  double linePointDistance(nrt::Point2D<T> const & A, nrt::Point2D<T> const & B, nrt::Point2D<T> const & C)
139  {
140  nrt::Point2D<double> const a(A);
141  nrt::Point2D<double> const b(B);
142  nrt::Point2D<double> const c(C);
143  return abs(cross(a,b,c) / Point2D<double>(a).distanceTo(Point2D<double>(b)));
144  }
145  }
146 }
147 
148 #define MOD(x) nrt::rotatedrectangledetails::mod(x, N)
149 template<class T>
151 {
152  std::vector<Point2D<T>> vertices = convexPoly.vertices();
153  if(vertices.size() == 0) return nrt::RotatedRectangle<T>();
154 
155  // Ensure that the vertex with the lowest y is first in the list
156  std::rotate(vertices.begin(),
157  std::min_element(vertices.begin(), vertices.end(),
158  [](nrt::Point2D<T> p1, nrt::Point2D<T> p2)
159  {
160  if(p1.y() == p2.y())
161  { return bool(p1.x() < p2.x()); }
162  else
163  { return bool(p1.y() < p2.y()); }
164  }),
165  vertices.end());
166 
167  int N = vertices.size();
168 
169  int minI = -1;
170  int minJ = -1;
171  int minK = -1;
172  int minM = -1;
173  double mind1 = -1;
174  double mind2 = -1;
175  int edge = -1;
176  double minArea = std::numeric_limits<double>::max();
177 
178  int j = 0;
179  int k = 0;
180  int m = 0;
181 
182  for(int i=0; i<N; ++i)
183  {
184  // Find a vertex on first perpendicular line of support
185  while(nrt::rotatedrectangledetails::DOTPR(i,j, vertices) > 0.0)
186  j = MOD(j + 1);
187 
188  // Find a vertex on parallel line of support
189  if(i == 0) k = j;
190  while(nrt::rotatedrectangledetails::CROSSPR(i,k, vertices) > 0.0)
191  k = MOD(k + 1);
192 
193  // Find a vertex on second perpendicular line of support
194  if(i == 0) m = k;
195  while(nrt::rotatedrectangledetails::DOTPR(i,m, vertices) < 0.0)
196  m = MOD(m + 1);
197 
198  // Find distances between parallel and perpendicular lines of support
199  double d1, d2;
200  if(vertices.at(MOD(i+1)).x() == vertices.at(i).x())
201  {
202  d1 = abs(vertices.at(k).x() - vertices.at(i).x());
203  d2 = abs(vertices.at(m).y() - vertices.at(j).y());
204  }
205  else if(vertices.at(MOD(i+1)).y() == vertices.at(i).y())
206  {
207  d1 = abs(vertices.at(k).y() - vertices.at(i).y());
208  d2 = abs(vertices.at(m).x() - vertices.at(j).x());
209  }
210  else
211  {
212  d1 = nrt::rotatedrectangledetails::linePointDistance(vertices.at(i), vertices.at(MOD(i+1)), vertices.at(k));
213 
214  double angle = nrt::Line<T>(vertices.at(i), vertices.at(MOD(i+1))).angle() + M_PI/2.0;
215  Point2D<double> Pj(vertices.at(j));
216  Point2D<double> Pjrot = Pj + Point2D<double>(cos(angle), sin(angle));
217 
218  d2 = nrt::rotatedrectangledetails::linePointDistance(Pj, Pjrot, Point2D<double>(vertices.at(m)));
219  }
220 
221  double A = d1 * d2;
222  if((i == 0 || A < minArea))
223  {
224  mind1 = d1;
225  mind2 = d2;
226  minArea = A;
227  edge = i;
228  minI = i;
229  minJ = j;
230  minK = k;
231  minM = m;
232  }
233  }
234 
235  // Reconstruct the bounding rectangle
236  double angle = Line<T>(vertices.at(MOD(edge)), vertices.at(MOD(edge+1))).angle();
237  angle = nrt::rotatedrectangledetails::mod(angle, M_PI);
238  Dims<T> dims(mind2, mind1);
239 
240  Point2D<double> c1(Point2D<double>(vertices.at(minI) + vertices.at(minK))/2.0);
241  Point2D<double> c2(c1 + Point2D<double>(cos(angle), sin(angle)));
242  Line<double> l1(c1, c2);
243 
244  Point2D<double> c3(Point2D<double>(vertices.at(minJ) + vertices.at(minM))/2.0);
245  Point2D<double> c4(c3 + Point2D<double>(cos(angle+M_PI/2.0), sin(angle+M_PI/2.0)));
246  Line<double> l2(c3, c4);
247 
248  Point2D<T> c(*l1.intersectInfinite<void>(l2));
249  if(dims.width() < dims.height())
250  {
251  dims = Dims<T>(dims.height(), dims.width());
252  angle = nrt::rotatedrectangledetails::mod(angle+M_PI/2.0, M_PI);
253  }
254  return RotatedRectangle<T>(c, dims, angle);
255 }
256 #undef MOD
257 
258 
259 // ######################################################################
260 // Free function implementations
261 // ######################################################################
262 template <class T> inline
263 std::ostream & nrt::operator<<(std::ostream & out, nrt::RotatedRectangle<T> const & r)
264 { return out << "[Center: " << r.topLeft() << ", Dims: " << r.dims() << ", Angle=" << r.angle() << "]";}
265 
266 // ######################################################################
267 template <class T> inline
268 nrt::RotatedRectangle<T> nrt::scale(nrt::RotatedRectangle<T> const & src, double const factor)
269 { return nrt::RotatedRectangle<T>(src.center(), src.dims() * factor, src.angle()); }
270 
271 // ######################################################################
272 template <class T> inline
273 nrt::RotatedRectangle<T> nrt::rotate(nrt::RotatedRectangle<T> const & src, double const angle)
274 { return nrt::RotatedRectangle<T>(src.center(), src.dims(), src.angle() + angle); }
275 
276 // ######################################################################
277 template <class T> inline
278 nrt::RotatedRectangle<T> nrt::rotateAbout(nrt::RotatedRectangle<T> const & src,
279  nrt::Point2D<T> const & point, double const angle)
280 { return nrt::RotatedRectangle<T>(nrt::rotateAbout(src.center(), point, angle), src.dims(), src.angle() + angle); }
281 
282 // ######################################################################
283 template <class T> inline
284 bool nrt::operator==(nrt::RotatedRectangle<T> const & lhs, nrt::RotatedRectangle<T> const & rhs)
285 {
286  return (lhs.center() == rhs.center() && lhs.dims() == rhs.dims && lhs.angle() == rhs.angle());
287 }
288 
289 // ######################################################################
290 template <class T> inline
291 bool nrt::operator!=(nrt::RotatedRectangle<T> const & lhs, nrt::RotatedRectangle<T> const & rhs)
292 { return ! (lhs == rhs); }
293 
294 // ######################################################################
295 template <class T> inline
297 { return nrt::RotatedRectangle<T>(lhs.center() + rhs, lhs.dims(), lhs.angle()); }
298 
299 // ######################################################################
300 template <class T> inline
302 { return nrt::RotatedRectangle<T>(lhs + rhs.center(), rhs.dims(), rhs.angle()); }
303 
304 // ######################################################################
305 template <class T> inline
307 { return nrt::RotatedRectangle<T>(lhs.center() - rhs, lhs.dims(), lhs.angle()); }
308 
309 // ######################################################################
310 template <class T> inline
312 { return nrt::RotatedRectangle<T>(lhs - rhs.center(), rhs.dims(), rhs.angle()); }
313 
314 // ######################################################################
315 template <class T> inline
317 { return nrt::RotatedRectangle<T>(lhs.center() * rhs, lhs.dims() * rhs, lhs.angle()); }
318 
319 // ######################################################################
320 template <class T> inline
322 { return nrt::RotatedRectangle<T>(lhs * rhs.center(), lhs * rhs.dims(), rhs.angle()); }
323 
324 // ######################################################################
325 template <class T> inline
327 { return nrt::RotatedRectangle<T>(lhs.center() / rhs, lhs.dims() / rhs, lhs.angle()); }
328 
329 // ######################################################################
330 template <class T> inline
332 { lhs = lhs * rhs; return lhs; }
333 
334 // ######################################################################
335 template <class T> inline
337 { lhs = lhs / rhs; return lhs; }
338 
339 // ######################################################################
340 template <class T> inline
342 { lhs = lhs + rhs; return lhs; }
343 
344 // ######################################################################
345 template <class T> inline
347 { lhs = lhs - rhs; return lhs; }