Box2D.cpp
Go to the documentation of this file.
1 //
2 // Box2D.cpp
3 //
4 
5 #define _USE_MATH_DEFINES
6 #include <cmath>
7 
8 #include "Box2D.hpp"
9 #include "../tools/errorhandler.hpp"
10 #include "Polygon2D.hpp"
11 #include <sstream>
12 
13 namespace datatypes
14 {
15 
16 
17 // ////////////////////////////////////////////////////////////
18 
19 // Define this macro so that checking of correct numeric ranges will
20 // be done at construction time of the box values.
21 //#define ASSERT_NUMERIC_RANGES
22 
24  : m_center(0, 0)
25  , m_size(0, 0)
26  , m_rotation(0)
27 {
29 }
30 
31 Box2D::Box2D(const Point2D& center, const Point2D& size, value_type rotation)
32  : m_center(center)
33  , m_size(size)
34  , m_rotation(rotation)
35 {
36  assert((m_size.getX() >= 0.0) || isNaN(m_size.getX()));
37  assert((m_size.getY() >= 0.0) || isNaN(m_size.getY()));
38 #ifdef ASSERT_NUMERIC_RANGES
40 #endif
42 }
43 
44 Box2D::Box2D(value_type x_center, value_type y_center, value_type x_size, value_type y_size, value_type rotation)
45  : m_center(x_center, y_center)
46  , m_size(x_size, y_size)
47  , m_rotation(rotation)
48 {
49  assert((m_size.getX() >= 0.0) || isNaN(m_size.getX()));
50  assert((m_size.getY() >= 0.0) || isNaN(m_size.getY()));
51 #ifdef ASSERT_NUMERIC_RANGES
53 #endif
55 }
56 
57 
58 void Box2D::setSize(const Point2D& p)
59 {
60  m_size = p;
61  verifyNumericRanges(); // not yet active to not break existing code!
62 }
63 void Box2D::setSize(value_type x_length, value_type y_width)
64 {
65  m_size.setXY(x_length, y_width);
66 #ifdef ASSERT_NUMERIC_RANGES
68 #endif
69 }
70 
72 {
73  m_rotation = r;
74 #ifdef ASSERT_NUMERIC_RANGES
76 #endif
77 }
78 
79 void Box2D::moveBy(const Point2D& pointvalues)
80 {
81  m_center += pointvalues;
82 }
83 
84 Box2D Box2D::movedBy(const Point2D& pointvalues) const
85 {
86  return Box2D(m_center + pointvalues, m_size, m_rotation);
87 }
88 
90 {
91  // This is the delta between center and edges
92  value_type deltaX = m_size.getX() * 0.5f;
93  value_type deltaY = m_size.getY() * 0.5f;
94 
95  // The cos/sin rotation factors
96  value_type dCos = std::cos(m_rotation);
97  value_type dSin = std::sin(m_rotation);
98 
99  // The dimensions after the rotation
100  value_type XCos = deltaX * dCos, XSin = deltaX * dSin;
101  value_type YCos = deltaY * dCos, YSin = deltaY * dSin;
102 
103  // Create the resulting points
104  Polygon2D poly;
105  poly.resize(5);
106  // Rotate each edge according to the m_rotation
107  poly[0] = m_center + Point2D( XCos - YSin, XSin + YCos);
108  poly[1] = m_center + Point2D( XCos + YSin, XSin - YCos);
109  poly[2] = m_center + Point2D( - XCos + YSin, - XSin - YCos);
110  poly[3] = m_center + Point2D( - XCos - YSin, - XSin + YCos);
111  poly[4] = poly[0];
112  return poly;
113 }
114 
116 {
117  // This is the delta between center and edges
118  value_type deltaX = m_size.getX() * 0.5f;
119  value_type deltaY = m_size.getY() * 0.5f;
120 
121  // The absolute values of the cos/sin rotation
122  value_type dCos = std::abs(std::cos(m_rotation));
123  value_type dSin = std::abs(std::sin(m_rotation));
124 
125  // The maximum x- and y-size of the rotated points. We use only
126  // absolute values because we want the maximum anyway.
127  value_type xmax = deltaX * dCos + deltaY * dSin;
128  value_type ymax = deltaX * dSin + deltaY * dCos;
129 
130  // The resulting size of the bounding box
131  Point2D size(2.0f * xmax, 2.0f * ymax);
132 
133  // Center stays identical, only size is changed
134  return Box2D(m_center, size, 0.0f);
135 }
136 
137 std::pair<Box2D::value_type, Box2D::value_type>
139 {
140  // This function calculates a low and a high boundary angle for
141  // all edges of the given (rotated) Box2D. The returned FloatPair
142  // has the component "first" for the lower bounding angle, and
143  // "second" for the upper bounding angle. (Note: This ordering is
144  // swapped compared to the scan point ordering!)
145 
146  // Need to check whether the origin is inside the box
147  if (containsPoint(Point2D(0, 0)))
148  {
149  // Origin is inside. Then return the full interval.
150  return std::make_pair(-PI, PI);
151  }
152 
153  // The usual case: The box does not contain the origin. Then we
154  // look for the min and max angles of the edges.
155 
156  Polygon2D poly = toPolygon();
157  value_type minangle = poly[0].angle();
158  value_type maxangle = minangle;
159  for (unsigned k = 1; k < 4; ++k)
160  {
161  value_type pointangle = poly[k].angle();
162 
163  if (pointangle < minangle)
164  minangle = pointangle;
165 
166  if (pointangle > maxangle)
167  maxangle = pointangle;
168  }
169 
170  return std::make_pair(minangle, maxangle);
171 }
172 
173 bool Box2D::containsPoint(const Point2D& point) const
174 {
175  if (fuzzyCompare(m_rotation, 0.0))
176  {
177  // Rotation is zero, hence we can directly check this in
178  // cartesian coordinates.
179  value_type pointX = point.getX() - m_center.getX();
180  value_type pointY = point.getY() - m_center.getY();
181 
182  // compare position to half size
183  return (std::abs(pointX) <= 0.5f * m_size.getX())
184  && (std::abs(pointY) <= 0.5f * m_size.getY());
185  }
186  else
187  {
188  // 2D coordinate transformation as proposed by Uni Ulm.
189 
190  // Move coordinate system to center of the box
191  value_type deltaX = point.getX() - m_center.getX();
192  value_type deltaY = point.getY() - m_center.getY();
193 
194  // If any of the X or Y components are outside of the
195  // "manhatten" diagonal bounding box, the point cannot be
196  // inside the box (and we don't even have to calculate the
197  // rotated point).
198  value_type half_sizeX = 0.5f * m_size.getX();
199  value_type half_sizeY = 0.5f * m_size.getY();
200  if (std::max(std::abs(deltaX), std::abs(deltaY)) > half_sizeX + half_sizeY)
201  return false;
202 
203  // Rotate by -psi
204  value_type dCos = std::cos(m_rotation);
205  value_type dSin = std::sin(m_rotation);
206  value_type pointX = dCos * deltaX + dSin * deltaY;
207  value_type pointY = -dSin * deltaX + dCos * deltaY;
208 
209  // compare position to half size
210  return ((std::abs(pointX) <= half_sizeX)
211  && (std::abs(pointY) <= half_sizeY));
212  }
213 }
214 
215 
216 // ////////////////////////////////////////////////////////////
217 
218 template<typename floatT>
219 static inline floatT
220 distanceFromStraightBox(floatT pointX, floatT pointY, floatT boxSizeX, floatT boxSizeY)
221 {
222  // Rotation is zero, hence we can directly check this in
223  // cartesian coordinates.
224  floatT pointXAbs = std::abs(pointX);
225  floatT pointYAbs = std::abs(pointY);
226 
227  // Above and right of the box?
228  if (pointXAbs > boxSizeX && pointYAbs > boxSizeY)
229  {
230  // Yes, return distance to edge
231  return hypot(pointXAbs - boxSizeX, pointYAbs - boxSizeY);
232  }
233  // Right of the box?
234  if (pointXAbs > boxSizeX)
235  // Yes, return the x-coordinate difference
236  return pointXAbs - boxSizeX;
237  // Above the box?
238  if (pointYAbs > boxSizeY)
239  // Yes, return the y-coordinate difference
240  return pointYAbs - boxSizeY;
241 
242  // We are obviously inside the box
243  return std::min(boxSizeX - pointXAbs, boxSizeY - pointYAbs);
244 }
245 
246 // Implementation of the distanceFromOutline algorithm.
247 template<class PointT>
248 static inline Box2D::value_type distanceFromOutline(const Box2D& box, const PointT& point)
249 {
250  Box2D::value_type boxSizeX = box.getSize().getX() * 0.5;
251  Box2D::value_type boxSizeY = box.getSize().getY() * 0.5;
252 
253  if (fuzzyCompare(box.getRotation(), 0.0))
254  {
255  // Rotation is zero, hence we can directly check this in
256  // cartesian coordinates.
257  return distanceFromStraightBox(point.getX() - box.getCenter().getX(),
258  point.getY() - box.getCenter().getY(),
259  boxSizeX, boxSizeY);
260  }
261  else
262  {
263  // 2D coordinate transformation
264 
265  // Move coordinate system to center of the box
266  Box2D::value_type deltaX = point.getX() - box.getCenter().getX();
267  Box2D::value_type deltaY = point.getY() - box.getCenter().getY();
268 
269  // Rotate by -psi
270  Box2D::value_type dCos = std::cos(box.getRotation());
271  Box2D::value_type dSin = std::sin(box.getRotation());
272  // and continue as if this were a straight box
273  return distanceFromStraightBox( dCos * deltaX + dSin * deltaY,
274  -dSin * deltaX + dCos * deltaY,
275  boxSizeX, boxSizeY);
276  }
277 }
278 
279 
280 // Calculate the mean of many distances. This is a template so that it
281 // works for both Point2D and Scanpoints.
282 template<class IteratorT>
283 static inline Box2D::value_type
284 distanceFromOutline(const Box2D& box, const IteratorT& begin, const IteratorT& end)
285 {
286  std::vector<Point2D>::size_type numPoints = 0;
287  Box2D::value_type result = 0.0;
288  for (IteratorT iter = begin; iter != end; ++iter)
289  {
290  numPoints++;
291  result += distanceFromOutline(box, *iter);
292  }
293  if (numPoints > 0)
294  {
295  return result / Box2D::value_type(numPoints);
296  }
297  else
298  {
299  return 0.0;
300  }
301 }
302 
303 
304 
305 // ////////////////////////////////////////////////////////////
306 
307 // Template function for the x-component of a rotational
308 // transform. This is a template because this works for both the
309 // Point2D and a ScanPoint.
310 template<class PointT>
311 static inline double rotate_x(const PointT& p, double dCos, double dSin)
312 {
313  return dCos * p.getX() + dSin * p.getY();
314 }
315 // Template function for the y-component of a rotational
316 // transform. This is a template because this works for both the
317 // Point2D and a ScanPoint.
318 template <class PointT>
319 static inline double rotate_y(const PointT& p, double dCos, double dSin)
320 {
321  return -dSin * p.getX() + dCos * p.getY();
322 }
323 
324 // Template function that implements the three overloaded methods
325 // below. A template function makes it very easy to enable using the
326 // vector<ScanPoint> as well, which is useful if we process actual
327 // scanpoints.
328 template<class IterT> static Box2D
329 calcOrientatedBox(double orientation, const IterT& begin, const IterT& end)
330 {
331  Box2D result;
332 
333  float dCos = cos(orientation);
334  float dSin = sin(orientation);
335 
336  // Is the sequence empty? If assertions are active, throw the
337  // assertion here. Otherwise silently return a null box.
338  assert(begin != end);
339  if (begin == end)
340  {
341  return result;
342  }
343 
344  // Set the min/max values to the coordinates of the first point
345  float minX = rotate_x(*begin, dCos, dSin);
346  float maxX = minX;
347  float minY = rotate_y(*begin, dCos, dSin);
348  float maxY = minY;
349 
350  // Iterate through the rest of the points to find the actual min
351  // and max values.
352  for (IterT iter = begin + 1; iter != end; iter++)
353  {
354  float x = rotate_x(*iter, dCos, dSin);
355  float y = rotate_y(*iter, dCos, dSin);
356  if (x < minX) minX = x;
357  if (x > maxX) maxX = x;
358  if (y < minY) minY = y;
359  if (y > maxY) maxY = y;
360  }
361 
362  // The center point is the mean of the bounds.
363  float rotated_center_x = 0.5 * (maxX + minX);
364  float rotated_center_y = 0.5 * (maxY + minY);
365 
366  // Rotate the center point back to the original coordinate system.
367  result.setCenter(dCos * rotated_center_x - dSin * rotated_center_y,
368  dSin * rotated_center_x + dCos * rotated_center_y);
369  // Size is even easier: Difference between max and min.
370  result.setSize(maxX - minX,
371  maxY - minY);
372  // Orientation was already given by the caller.
373  result.setRotation(orientation);
374 
375  return result;
376 }
377 
378 // Returns an orientated bounding box for the given list of points.
380 {
381  return calcOrientatedBox(orientation, poly.begin(), poly.end());
382 }
383 
384 // Returns an orientated bounding box for the given list of points.
386  const std::vector<Point2D>::const_iterator& begin,
387  const std::vector<Point2D>::const_iterator& end)
388 {
389  return calcOrientatedBox(orientation, begin, end);
390 }
391 
392 
394  const std::vector<Point2D>& points)
395 {
396  return calcOrientatedBox(orientation_rad, points.begin(), points.end());
397 }
398 
399 // Returns an orientated bounding box for the given list of points.
400 // Box2D Box2D::orientatedBox(value_type orientation,
401 // const std::vector<ScanPoint>::const_iterator& begin,
402 // const std::vector<ScanPoint>::const_iterator& end)
403 // {
404 // return calcOrientatedBox(orientation, begin, end);
405 // }
406 //
407 // Box2D Box2D::orientatedBox(value_type orientation_rad,
408 // const SegmentIterator& begin,
409 // const SegmentIterator& end)
410 // {
411 // return calcOrientatedBox(orientation_rad, begin, end);
412 // }
413 
414 
415 
416 
417 
419 {
420 #ifdef __GNUC__
421  // Allow correctRange to be set but unused in case everything works well.
422  // This is needed for gcc v4.6 and up when using -Wall
423  bool __attribute__((unused)) correctRange = true;
424 #else
425  bool correctRange = true;
426 #endif
427  // Check our assumptions about the m_size
428  if (m_size.getX() < 0 || m_size.getY() < 0)
429  {
430  printWarning("Size of Box2D was given as negative value (" +
431  ::toString(m_size.getX(), 2) + "," + ::toString(m_size.getY(), 2) +
432  ") - silently using the absolute value instead.");
433  m_size.setX(std::abs(m_size.getX()));
434  m_size.setY(std::abs(m_size.getY()));
435  correctRange = false;
436  // This is probably better than throwing an exception.
437  //throw std::out_of_range("Size of Box2D negative - this is an invalid Box2D.");
438  }
439 
440  if (isNaN(m_size.getX()))
441  {
442  m_size.setX(0);
443  printWarning("Size.getX() of Box2D was given as NaN value - silently using Zero instead.");
444  correctRange = false;
445  }
446  if (isNaN(m_size.getY()))
447  {
448  m_size.setY(0);
449  printWarning("Size.getY() of Box2D was given as NaN value - silently using Zero instead.");
450  correctRange = false;
451  }
452  if (isNaN(m_rotation))
453  {
454  m_rotation = 0;
455  printWarning("Rotation of Box2D was given as NaN value - silently using Zero instead.");
456  correctRange = false;
457  }
459  if (!fuzzyCompare(normd_rot, m_rotation))
460  {
461  printWarning("Rotation of Box2D (" + ::toString(m_rotation, 2) + ") was outside of its [-pi,pi] definition range - silently normalizing it back into that range (" +
462  ::toString(normd_rot, 2) + ").");
463  m_rotation = normd_rot;
464  correctRange = false;
465  }
466 #ifdef ASSERT_NUMERIC_RANGES
467  assert(correctRange);
468 #endif
469 }
470 
471 std::string Box2D::toString() const
472 {
473  std::string text;
474 
475  text = "[ Center=" + getCenter().toString() +
476  " Size=" + getSize().toString() +
477  " Rotation=" + ::toString(getRotation(), 2) +
478  "]";
479 
480  return text;
481 }
482 
483 } // namespace datatypes
484 
value_type getRotation() const
Definition: Box2D.hpp:108
value_type m_rotation
Definition: Box2D.hpp:42
std::string toString() const
Definition: Box2D.cpp:471
std::string toString(UINT16 digits=2) const
Text output for debugging.
Definition: Point2D.cpp:75
const Point2D & getCenter() const
Returns the center point of this Box.
Definition: Box2D.hpp:92
const Point2D & getSize() const
Returns the size of this Box.
Definition: Box2D.hpp:101
value_type getX() const
Definition: Point2D.hpp:70
A rotated 2-dimensional box in the plane.
Definition: Box2D.hpp:34
void setSize(const Point2D &p)
Sets the size of this Box. Must be non-negative.
Definition: Box2D.cpp:58
double hypot(double x, double y, double z)
Definition: MathToolbox.cpp:19
A polygon of 2D-points.
Definition: Polygon2D.hpp:43
std::pair< value_type, value_type > getBoundingAngles() const
Returns boundary angles for this box.
Definition: Box2D.cpp:138
static double rotate_y(const PointT &p, double dCos, double dSin)
Definition: Box2D.cpp:319
Box2D movedBy(const Point2D &centerMovement) const
Returns a Box that is copied from this one but with its center point moved.
Definition: Box2D.cpp:84
Polygon2D toPolygon() const
Converts this Box2D to a closed polygon.
Definition: Box2D.cpp:89
static Box2D orientatedBox(value_type orientation_rad, const Polygon2D &poly)
Returns an orientated bounding box for the given list of points.
Definition: Box2D.cpp:379
void setX(value_type x)
Definition: Point2D.hpp:140
value_type getY() const
Definition: Point2D.hpp:73
static double rotate_x(const PointT &p, double dCos, double dSin)
Definition: Box2D.cpp:311
Point2D::value_type distanceFromOutline(const Point2D &point) const
Returns the distance from the outline of this Box2D to the given point.
Point2D::value_type value_type
The type of the stored x, y coordinates, and the rotation.
Definition: Box2D.hpp:38
static floatT distanceFromStraightBox(floatT pointX, floatT pointY, floatT boxSizeX, floatT boxSizeY)
Definition: Box2D.cpp:220
void setRotation(value_type r)
Sets the rotation angle of this Box in [radians], counter clock wise.
Definition: Box2D.cpp:71
Point2D m_size
Definition: Box2D.hpp:41
void setCenter(const Point2D &p)
Sets the center point of this Box2D.
Definition: Box2D.hpp:148
void setXY(value_type x, value_type y)
Definition: Point2D.hpp:147
Box2D toBoundingBox() const
Returns a Box in parallel to the coordinate system that bounds this box.
Definition: Box2D.cpp:115
Box2D()
Constructor for an all-zero Box2D.
Definition: Box2D.cpp:23
bool fuzzyCompare(double a, double b)
Tests if two double values are nearly equal.
Definition: MathToolbox.hpp:28
double normalizeRadians(double radians)
Definition: MathToolbox.cpp:31
bool containsPoint(const Point2D &point) const
Returns true if the given Point2D is inside this box or on its outline.
Definition: Box2D.cpp:173
void setY(value_type y)
Definition: Point2D.hpp:143
Point2D m_center
Definition: Box2D.hpp:40
bool isNaN(floatT x)
Checks if a floating point value is Not-a-Number (NaN)
Definition: MathToolbox.hpp:63
static Box2D calcOrientatedBox(double orientation, const IterT &begin, const IterT &end)
Definition: Box2D.cpp:329
void printWarning(std::string message)
#define PI
void moveBy(const Point2D &centerMovement)
Move the center point of this box by the given point values.
Definition: Box2D.cpp:79
void verifyNumericRanges()
Definition: Box2D.cpp:418


libsick_ldmrs
Author(s): SICK AG , Martin Günther , Jochen Sprickerhof
autogenerated on Sat Jun 8 2019 17:57:33