Box2D.cpp
Go to the documentation of this file.
00001 //
00002 // Box2D.cpp
00003 //
00004 
00005 #define _USE_MATH_DEFINES
00006 #include <cmath>
00007 
00008 #include "Box2D.hpp"
00009 #include "../tools/errorhandler.hpp"
00010 #include "Polygon2D.hpp"
00011 #include <sstream>
00012 
00013 namespace datatypes
00014 {
00015 
00016 
00017 // ////////////////////////////////////////////////////////////
00018 
00019 // Define this macro so that checking of correct numeric ranges will
00020 // be done at construction time of the box values.
00021 //#define ASSERT_NUMERIC_RANGES
00022 
00023 Box2D::Box2D()
00024         : m_center(0, 0)
00025         , m_size(0, 0)
00026         , m_rotation(0)
00027 {
00028         m_datatype = Datatype_Box2D;
00029 }
00030 
00031 Box2D::Box2D(const Point2D& center, const Point2D& size, value_type rotation)
00032         : m_center(center)
00033         , m_size(size)
00034         , m_rotation(rotation)
00035 {
00036         assert((m_size.getX() >= 0.0) || isNaN(m_size.getX()));
00037         assert((m_size.getY() >= 0.0) || isNaN(m_size.getY()));
00038 #ifdef ASSERT_NUMERIC_RANGES
00039         verifyNumericRanges();
00040 #endif
00041         m_datatype = Datatype_Box2D;
00042 }
00043 
00044 Box2D::Box2D(value_type x_center, value_type y_center, value_type x_size, value_type y_size, value_type rotation)
00045         : m_center(x_center, y_center)
00046         , m_size(x_size, y_size)
00047         , m_rotation(rotation)
00048 {
00049         assert((m_size.getX() >= 0.0) || isNaN(m_size.getX()));
00050         assert((m_size.getY() >= 0.0) || isNaN(m_size.getY()));
00051 #ifdef ASSERT_NUMERIC_RANGES
00052         verifyNumericRanges();
00053 #endif
00054         m_datatype = Datatype_Box2D;
00055 }
00056 
00057 
00058 void Box2D::setSize(const Point2D& p)
00059 {
00060         m_size = p;
00061         verifyNumericRanges(); // not yet active to not break existing code!
00062 }
00063 void Box2D::setSize(value_type x_length, value_type y_width)
00064 {
00065         m_size.setXY(x_length, y_width);
00066 #ifdef ASSERT_NUMERIC_RANGES
00067         verifyNumericRanges();
00068 #endif
00069 }
00070 
00071 void Box2D::setRotation(value_type r)
00072 {
00073         m_rotation = r;
00074 #ifdef ASSERT_NUMERIC_RANGES
00075         verifyNumericRanges();
00076 #endif
00077 }
00078 
00079 void Box2D::moveBy(const Point2D& pointvalues)
00080 {
00081         m_center += pointvalues;
00082 }
00083 
00084 Box2D Box2D::movedBy(const Point2D& pointvalues) const
00085 {
00086         return Box2D(m_center + pointvalues, m_size, m_rotation);
00087 }
00088 
00089 Polygon2D Box2D::toPolygon() const
00090 {
00091         // This is the delta between center and edges
00092         value_type deltaX = m_size.getX() * 0.5f;
00093         value_type deltaY = m_size.getY() * 0.5f;
00094 
00095         // The cos/sin rotation factors
00096         value_type dCos = std::cos(m_rotation);
00097         value_type dSin = std::sin(m_rotation);
00098 
00099         // The dimensions after the rotation
00100         value_type XCos = deltaX * dCos, XSin = deltaX * dSin;
00101         value_type YCos = deltaY * dCos, YSin = deltaY * dSin;
00102 
00103         // Create the resulting points
00104         Polygon2D poly;
00105         poly.resize(5);
00106         // Rotate each edge according to the m_rotation
00107         poly[0] = m_center + Point2D(   XCos - YSin,    XSin + YCos);
00108         poly[1] = m_center + Point2D(   XCos + YSin,    XSin - YCos);
00109         poly[2] = m_center + Point2D( - XCos + YSin,  - XSin - YCos);
00110         poly[3] = m_center + Point2D( - XCos - YSin,  - XSin + YCos);
00111         poly[4] = poly[0];
00112         return poly;
00113 }
00114 
00115 Box2D Box2D::toBoundingBox() const
00116 {
00117         // This is the delta between center and edges
00118         value_type deltaX = m_size.getX() * 0.5f;
00119         value_type deltaY = m_size.getY() * 0.5f;
00120 
00121         // The absolute values of the cos/sin rotation
00122         value_type dCos = std::abs(std::cos(m_rotation));
00123         value_type dSin = std::abs(std::sin(m_rotation));
00124 
00125         // The maximum x- and y-size of the rotated points. We use only
00126         // absolute values because we want the maximum anyway.
00127         value_type xmax = deltaX * dCos + deltaY * dSin;
00128         value_type ymax = deltaX * dSin + deltaY * dCos;
00129 
00130         // The resulting size of the bounding box
00131         Point2D size(2.0f * xmax, 2.0f * ymax);
00132 
00133         // Center stays identical, only size is changed
00134         return Box2D(m_center, size, 0.0f);
00135 }
00136 
00137 std::pair<Box2D::value_type, Box2D::value_type>
00138 Box2D::getBoundingAngles() const
00139 {
00140         // This function calculates a low and a high boundary angle for
00141         // all edges of the given (rotated) Box2D. The returned FloatPair
00142         // has the component "first" for the lower bounding angle, and
00143         // "second" for the upper bounding angle. (Note: This ordering is
00144         // swapped compared to the scan point ordering!)
00145 
00146         // Need to check whether the origin is inside the box
00147         if (containsPoint(Point2D(0, 0)))
00148         {
00149                 // Origin is inside. Then return the full interval.
00150                 return std::make_pair(-PI, PI);
00151         }
00152 
00153         // The usual case: The box does not contain the origin. Then we
00154         // look for the min and max angles of the edges.
00155 
00156         Polygon2D poly = toPolygon();
00157         value_type minangle = poly[0].angle();
00158         value_type maxangle = minangle;
00159         for (unsigned k = 1; k < 4; ++k)
00160         {
00161                 value_type pointangle = poly[k].angle();
00162 
00163                 if (pointangle < minangle)
00164                         minangle = pointangle;
00165 
00166                 if (pointangle > maxangle)
00167                         maxangle = pointangle;
00168         }
00169 
00170         return std::make_pair(minangle, maxangle);
00171 }
00172 
00173 bool Box2D::containsPoint(const Point2D& point) const
00174 {
00175         if (fuzzyCompare(m_rotation, 0.0))
00176         {
00177                 // Rotation is zero, hence we can directly check this in
00178                 // cartesian coordinates.
00179                 value_type pointX = point.getX() - m_center.getX();
00180                 value_type pointY = point.getY() - m_center.getY();
00181 
00182                 // compare position to half size
00183                 return (std::abs(pointX) <=  0.5f * m_size.getX())
00184                            && (std::abs(pointY) <=  0.5f * m_size.getY());
00185         }
00186         else
00187         {
00188                 // 2D coordinate transformation as proposed by Uni Ulm.
00189 
00190                 // Move coordinate system to center of the box
00191                 value_type deltaX = point.getX() - m_center.getX();
00192                 value_type deltaY = point.getY() - m_center.getY();
00193 
00194                 // If any of the X or Y components are outside of the
00195                 // "manhatten" diagonal bounding box, the point cannot be
00196                 // inside the box (and we don't even have to calculate the
00197                 // rotated point).
00198                 value_type half_sizeX = 0.5f * m_size.getX();
00199                 value_type half_sizeY = 0.5f * m_size.getY();
00200                 if (std::max(std::abs(deltaX), std::abs(deltaY)) > half_sizeX + half_sizeY)
00201                         return false;
00202 
00203                 // Rotate by -psi
00204                 value_type dCos = std::cos(m_rotation);
00205                 value_type dSin = std::sin(m_rotation);
00206                 value_type pointX =  dCos * deltaX  +  dSin * deltaY;
00207                 value_type pointY = -dSin * deltaX  +  dCos * deltaY;
00208 
00209                 // compare position to half size
00210                 return ((std::abs(pointX) <=  half_sizeX)
00211                                 && (std::abs(pointY) <=  half_sizeY));
00212         }
00213 }
00214 
00215 
00216 // ////////////////////////////////////////////////////////////
00217 
00218 template<typename floatT>
00219 static inline floatT
00220 distanceFromStraightBox(floatT pointX, floatT pointY, floatT boxSizeX, floatT boxSizeY)
00221 {
00222         // Rotation is zero, hence we can directly check this in
00223         // cartesian coordinates.
00224         floatT pointXAbs = std::abs(pointX);
00225         floatT pointYAbs = std::abs(pointY);
00226 
00227         // Above and right of the box?
00228         if (pointXAbs > boxSizeX && pointYAbs > boxSizeY)
00229         {
00230                 // Yes, return distance to edge
00231                 return hypot(pointXAbs - boxSizeX, pointYAbs - boxSizeY);
00232         }
00233         // Right of the box?
00234         if (pointXAbs > boxSizeX)
00235                 // Yes, return the x-coordinate difference
00236                 return pointXAbs - boxSizeX;
00237         // Above the box?
00238         if (pointYAbs > boxSizeY)
00239                 // Yes, return the y-coordinate difference
00240                 return pointYAbs - boxSizeY;
00241 
00242         // We are obviously inside the box
00243         return std::min(boxSizeX - pointXAbs, boxSizeY - pointYAbs);
00244 }
00245 
00246 // Implementation of the distanceFromOutline algorithm.
00247 template<class PointT>
00248 static inline Box2D::value_type distanceFromOutline(const Box2D& box, const PointT& point)
00249 {
00250         Box2D::value_type boxSizeX = box.getSize().getX() * 0.5;
00251         Box2D::value_type boxSizeY = box.getSize().getY() * 0.5;
00252 
00253         if (fuzzyCompare(box.getRotation(), 0.0))
00254         {
00255                 // Rotation is zero, hence we can directly check this in
00256                 // cartesian coordinates.
00257                 return distanceFromStraightBox(point.getX() - box.getCenter().getX(),
00258                                                                            point.getY() - box.getCenter().getY(),
00259                                                                            boxSizeX, boxSizeY);
00260         }
00261         else
00262         {
00263                 // 2D coordinate transformation
00264 
00265                 // Move coordinate system to center of the box
00266                 Box2D::value_type deltaX = point.getX() - box.getCenter().getX();
00267                 Box2D::value_type deltaY = point.getY() - box.getCenter().getY();
00268 
00269                 // Rotate by -psi
00270                 Box2D::value_type dCos = std::cos(box.getRotation());
00271                 Box2D::value_type dSin = std::sin(box.getRotation());
00272                 // and continue as if this were a straight box
00273                 return distanceFromStraightBox( dCos * deltaX  +  dSin * deltaY,
00274                                                                                 -dSin * deltaX  +  dCos * deltaY,
00275                                                                                 boxSizeX, boxSizeY);
00276         }
00277 }
00278 
00279 
00280 // Calculate the mean of many distances. This is a template so that it
00281 // works for both Point2D and Scanpoints.
00282 template<class IteratorT>
00283 static inline Box2D::value_type
00284 distanceFromOutline(const Box2D& box, const IteratorT& begin, const IteratorT& end)
00285 {
00286         std::vector<Point2D>::size_type numPoints = 0;
00287         Box2D::value_type result = 0.0;
00288         for (IteratorT iter = begin; iter != end; ++iter)
00289         {
00290                 numPoints++;
00291                 result += distanceFromOutline(box, *iter);
00292         }
00293         if (numPoints > 0)
00294         {
00295                 return result / Box2D::value_type(numPoints);
00296         }
00297         else
00298         {
00299                 return 0.0;
00300         }
00301 }
00302 
00303 
00304 
00305 // ////////////////////////////////////////////////////////////
00306 
00307 // Template function for the x-component of a rotational
00308 // transform. This is a template because this works for both the
00309 // Point2D and a ScanPoint.
00310 template<class PointT>
00311 static inline double rotate_x(const PointT& p, double dCos, double dSin)
00312 {
00313         return dCos * p.getX() + dSin * p.getY();
00314 }
00315 // Template function for the y-component of a rotational
00316 // transform. This is a template because this works for both the
00317 // Point2D and a ScanPoint.
00318 template <class PointT>
00319 static inline double rotate_y(const PointT& p, double dCos, double dSin)
00320 {
00321         return -dSin * p.getX() + dCos * p.getY();
00322 }
00323 
00324 // Template function that implements the three overloaded methods
00325 // below. A template function makes it very easy to enable using the
00326 // vector<ScanPoint> as well, which is useful if we process actual
00327 // scanpoints.
00328 template<class IterT> static Box2D
00329 calcOrientatedBox(double orientation, const IterT& begin, const IterT& end)
00330 {
00331         Box2D result;
00332 
00333         float dCos = cos(orientation);
00334         float dSin = sin(orientation);
00335 
00336         // Is the sequence empty? If assertions are active, throw the
00337         // assertion here. Otherwise silently return a null box.
00338         assert(begin != end);
00339         if (begin == end)
00340         {
00341                 return result;
00342         }
00343 
00344         // Set the min/max values to the coordinates of the first point
00345         float minX = rotate_x(*begin, dCos, dSin);
00346         float maxX = minX;
00347         float minY = rotate_y(*begin, dCos, dSin);
00348         float maxY = minY;
00349 
00350         // Iterate through the rest of the points to find the actual min
00351         // and max values.
00352         for (IterT iter = begin + 1; iter != end; iter++)
00353         {
00354                 float x = rotate_x(*iter, dCos, dSin);
00355                 float y = rotate_y(*iter, dCos, dSin);
00356                 if (x < minX) minX = x;
00357                 if (x > maxX) maxX = x;
00358                 if (y < minY) minY = y;
00359                 if (y > maxY) maxY = y;
00360         }
00361 
00362         // The center point is the mean of the bounds.
00363         float rotated_center_x = 0.5 * (maxX + minX);
00364         float rotated_center_y = 0.5 * (maxY + minY);
00365 
00366         // Rotate the center point back to the original coordinate system.
00367         result.setCenter(dCos * rotated_center_x - dSin * rotated_center_y,
00368                                          dSin * rotated_center_x + dCos * rotated_center_y);
00369         // Size is even easier: Difference between max and min.
00370         result.setSize(maxX - minX,
00371                                    maxY - minY);
00372         // Orientation was already given by the caller.
00373         result.setRotation(orientation);
00374 
00375         return result;
00376 }
00377 
00378 // Returns an orientated bounding box for the given list of points.
00379 Box2D Box2D::orientatedBox(value_type orientation, const Polygon2D& poly)
00380 {
00381         return calcOrientatedBox(orientation, poly.begin(), poly.end());
00382 }
00383 
00384 // Returns an orientated bounding box for the given list of points.
00385 Box2D Box2D::orientatedBox(value_type orientation,
00386                                                    const std::vector<Point2D>::const_iterator& begin,
00387                                                    const std::vector<Point2D>::const_iterator& end)
00388 {
00389         return calcOrientatedBox(orientation, begin, end);
00390 }
00391 
00392 
00393 Box2D Box2D::orientatedBox(value_type orientation_rad,
00394                                                    const std::vector<Point2D>& points)
00395 {
00396         return calcOrientatedBox(orientation_rad, points.begin(), points.end());
00397 }
00398 
00399 // Returns an orientated bounding box for the given list of points.
00400 // Box2D Box2D::orientatedBox(value_type orientation,
00401 //                                                 const std::vector<ScanPoint>::const_iterator& begin,
00402 //                                                 const std::vector<ScanPoint>::const_iterator& end)
00403 // {
00404 //      return calcOrientatedBox(orientation, begin, end);
00405 // }
00406 // 
00407 // Box2D Box2D::orientatedBox(value_type orientation_rad,
00408 //                                                 const SegmentIterator& begin,
00409 //                                                 const SegmentIterator& end)
00410 // {
00411 //      return calcOrientatedBox(orientation_rad, begin, end);
00412 // }
00413 
00414 
00415 
00416 
00417 
00418 void Box2D::verifyNumericRanges()
00419 {
00420 #ifdef __GNUC__
00421         // Allow correctRange to be set but unused in case everything works well.
00422         // This is needed for gcc v4.6 and up when using -Wall
00423         bool __attribute__((unused)) correctRange = true;
00424 #else
00425         bool correctRange = true;
00426 #endif
00427         // Check our assumptions about the m_size
00428         if (m_size.getX() < 0 || m_size.getY() < 0)
00429         {
00430                 printWarning("Size of Box2D was given as negative value (" +
00431                                                  ::toString(m_size.getX(), 2) + "," + ::toString(m_size.getY(), 2) +
00432                                                  ") - silently using the absolute value instead.");
00433                 m_size.setX(std::abs(m_size.getX()));
00434                 m_size.setY(std::abs(m_size.getY()));
00435                 correctRange = false;
00436                 // This is probably better than throwing an exception.
00437                 //throw std::out_of_range("Size of Box2D negative - this is an invalid Box2D.");
00438         }
00439 
00440         if (isNaN(m_size.getX()))
00441         {
00442                 m_size.setX(0);
00443                 printWarning("Size.getX() of Box2D was given as NaN value - silently using Zero instead.");
00444                 correctRange = false;
00445         }
00446         if (isNaN(m_size.getY()))
00447         {
00448                 m_size.setY(0);
00449                 printWarning("Size.getY() of Box2D was given as NaN value - silently using Zero instead.");
00450                 correctRange = false;
00451         }
00452         if (isNaN(m_rotation))
00453         {
00454                 m_rotation = 0;
00455                 printWarning("Rotation of Box2D was given as NaN value - silently using Zero instead.");
00456                 correctRange = false;
00457         }
00458         value_type normd_rot = normalizeRadians(m_rotation);
00459         if (!fuzzyCompare(normd_rot, m_rotation))
00460         {
00461                 printWarning("Rotation of Box2D (" + ::toString(m_rotation, 2) + ") was outside of its [-pi,pi] definition range - silently normalizing it back into that range (" +
00462                                                         ::toString(normd_rot, 2) + ").");
00463                 m_rotation = normd_rot;
00464                 correctRange = false;
00465         }
00466 #ifdef ASSERT_NUMERIC_RANGES
00467         assert(correctRange);
00468 #endif
00469 }
00470 
00471 std::string Box2D::toString() const
00472 {
00473         std::string text;
00474         
00475         text = "[ Center=" + getCenter().toString() +
00476                         " Size=" + getSize().toString() +
00477                         " Rotation=" + ::toString(getRotation(), 2) +
00478                         "]";
00479         
00480         return text;
00481 }
00482 
00483 }       // namespace datatypes
00484 


libsick_ldmrs
Author(s): SICK AG , Martin Günther , Jochen Sprickerhof
autogenerated on Wed Jun 14 2017 04:04:50