bodies.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #include "geometric_shapes/bodies.h"
00038 #include "geometric_shapes/body_operations.h"
00039 
00040 #include <console_bridge/console.h>
00041 
00042 extern "C"
00043 {
00044 #ifdef GEOMETRIC_SHAPES_HAVE_QHULL_2011
00045 #include <libqhull/libqhull.h>
00046 #include <libqhull/mem.h>
00047 #include <libqhull/qset.h>
00048 #include <libqhull/geom.h>
00049 #include <libqhull/merge.h>
00050 #include <libqhull/poly.h>
00051 #include <libqhull/io.h>
00052 #include <libqhull/stat.h>
00053 #else
00054 #include <qhull/qhull.h>
00055 #include <qhull/mem.h>
00056 #include <qhull/qset.h>
00057 #include <qhull/geom.h>
00058 #include <qhull/merge.h>
00059 #include <qhull/poly.h>
00060 #include <qhull/io.h>
00061 #include <qhull/stat.h>
00062 #endif
00063 }
00064 
00065 #include <boost/math/constants/constants.hpp>
00066 #include <limits>
00067 #include <cstdio>
00068 #include <algorithm>
00069 #include <Eigen/Geometry>
00070 
00071 namespace bodies
00072 {
00073 namespace detail
00074 {
00075 static const double ZERO = 1e-9;
00076 
00079 static inline double distanceSQR(const Eigen::Vector3d& p, const Eigen::Vector3d& origin, const Eigen::Vector3d& dir)
00080 {
00081   Eigen::Vector3d a = p - origin;
00082   double d = dir.dot(a);
00083   return a.squaredNorm() - d * d;
00084 }
00085 
00086 // temp structure for intersection points (used for ordering them)
00087 struct intersc
00088 {
00089   intersc(const Eigen::Vector3d &_pt, const double _tm) : pt(_pt), time(_tm) {}
00090 
00091   Eigen::Vector3d pt;
00092   double          time;
00093 
00094   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00095 };
00096 
00097 // define order on intersection points
00098 struct interscOrder
00099 {
00100   bool operator()(const intersc &a, const intersc &b) const
00101   {
00102     return a.time < b.time;
00103   }
00104 };
00105 }
00106 }
00107 
00108 void bodies::Body::setDimensions(const shapes::Shape *shape)
00109 {
00110   useDimensions(shape);
00111   updateInternalData();
00112 }
00113 
00114 bool bodies::Body::samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result)
00115 {
00116   BoundingSphere bs;
00117   computeBoundingSphere(bs);
00118   for (unsigned int i = 0 ; i < max_attempts ; ++i)
00119   {
00120     result = Eigen::Vector3d(rng.uniformReal(bs.center.x() - bs.radius, bs.center.x() + bs.radius),
00121                              rng.uniformReal(bs.center.y() - bs.radius, bs.center.y() + bs.radius),
00122                              rng.uniformReal(bs.center.z() - bs.radius, bs.center.z() + bs.radius));
00123     if (containsPoint(result))
00124       return true;
00125   }
00126   return false;
00127 }
00128 
00129 bool bodies::Sphere::containsPoint(const Eigen::Vector3d &p, bool verbose) const
00130 {
00131   return (center_ - p).squaredNorm() < radius2_;
00132 }
00133 
00134 void bodies::Sphere::useDimensions(const shapes::Shape *shape) // radius
00135 {
00136   radius_ = static_cast<const shapes::Sphere*>(shape)->radius;
00137 }
00138 
00139 std::vector<double> bodies::Sphere::getDimensions() const
00140 {
00141   std::vector<double> d(1, radius_);
00142   return d;
00143 }
00144 
00145 void bodies::Sphere::updateInternalData()
00146 {
00147   radiusU_ = radius_ * scale_ + padding_;
00148   radius2_ = radiusU_ * radiusU_;
00149   center_ = pose_.translation();
00150 }
00151 
00152 boost::shared_ptr<bodies::Body> bodies::Sphere::cloneAt(const Eigen::Affine3d &pose, double padding, double scale) const
00153 {
00154   Sphere *s = new Sphere();
00155   s->radius_ = radius_;
00156   s->padding_ = padding;
00157   s->scale_ = scale;
00158   s->pose_ = pose;
00159   s->updateInternalData();
00160   return boost::shared_ptr<Body>(s);
00161 }
00162 
00163 double bodies::Sphere::computeVolume() const
00164 {
00165   return 4.0 * boost::math::constants::pi<double>() * radiusU_ * radiusU_ * radiusU_ / 3.0;
00166 }
00167 
00168 void bodies::Sphere::computeBoundingSphere(BoundingSphere &sphere) const
00169 {
00170   sphere.center = center_;
00171   sphere.radius = radiusU_;
00172 }
00173 
00174 void bodies::Sphere::computeBoundingCylinder(BoundingCylinder &cylinder) const
00175 {
00176   cylinder.pose = pose_;
00177   cylinder.radius = radiusU_;
00178   cylinder.length = radiusU_;
00179 
00180 }
00181 
00182 bool bodies::Sphere::samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result)
00183 {
00184   for (unsigned int i = 0 ; i < max_attempts ; ++i)
00185   {
00186     const double minX = center_.x() - radiusU_; const double maxX = center_.x() + radiusU_;
00187     const double minY = center_.y() - radiusU_; const double maxY = center_.y() + radiusU_;
00188     const double minZ = center_.z() - radiusU_; const double maxZ = center_.z() + radiusU_;
00189     // we are sampling in a box; the probability of success after 20 attempts is 99.99996% given the ratio of box volume to sphere volume
00190     for (int j = 0 ; j < 20 ; ++j)
00191     {
00192       result = Eigen::Vector3d(rng.uniformReal(minX, maxX),
00193                                rng.uniformReal(minY, maxY),
00194                                rng.uniformReal(minZ, maxZ));
00195       if (containsPoint(result))
00196         return true;
00197     }
00198   }
00199   return false;
00200 }
00201 
00202 bool bodies::Sphere::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d *intersections, unsigned int count) const
00203 {
00204   if (detail::distanceSQR(center_, origin, dir) > radius2_) return false;
00205 
00206   bool result = false;
00207 
00208   Eigen::Vector3d cp = origin - center_;
00209   double dpcpv = cp.dot(dir);
00210 
00211   Eigen::Vector3d w = cp - dpcpv * dir;
00212   Eigen::Vector3d Q = center_ + w;
00213   double x = radius2_ - w.squaredNorm();
00214 
00215   if (fabs(x) < detail::ZERO)
00216   {
00217     w = Q - origin;
00218     double dpQv = w.dot(dir);
00219     if (dpQv > detail::ZERO)
00220     {
00221       if (intersections)
00222         intersections->push_back(Q);
00223       result = true;
00224     }
00225   } else
00226     if (x > 0.0)
00227     {
00228       x = sqrt(x);
00229       w = dir * x;
00230       Eigen::Vector3d A = Q - w;
00231       Eigen::Vector3d B = Q + w;
00232       w = A - origin;
00233       double dpAv = w.dot(dir);
00234       w = B - origin;
00235       double dpBv = w.dot(dir);
00236 
00237       if (dpAv > detail::ZERO)
00238       {
00239         result = true;
00240         if (intersections)
00241         {
00242           intersections->push_back(A);
00243           if (count == 1)
00244             return result;
00245         }
00246       }
00247 
00248       if (dpBv > detail::ZERO)
00249       {
00250         result = true;
00251         if (intersections)
00252           intersections->push_back(B);
00253       }
00254     }
00255   return result;
00256 }
00257 
00258 bool bodies::Cylinder::containsPoint(const Eigen::Vector3d &p, bool verbose) const
00259 {
00260   Eigen::Vector3d v = p - center_;
00261   double pH = v.dot(normalH_);
00262 
00263   if (fabs(pH) > length2_)
00264     return false;
00265 
00266   double pB1 = v.dot(normalB1_);
00267   double remaining = radius2_ - pB1 * pB1;
00268 
00269   if (remaining < 0.0)
00270     return false;
00271   else
00272   {
00273     double pB2 = v.dot(normalB2_);
00274     return pB2 * pB2 < remaining;
00275   }
00276 }
00277 
00278 void bodies::Cylinder::useDimensions(const shapes::Shape *shape) // (length, radius)
00279 {
00280   length_ = static_cast<const shapes::Cylinder*>(shape)->length;
00281   radius_ = static_cast<const shapes::Cylinder*>(shape)->radius;
00282 }
00283 
00284 std::vector<double> bodies::Cylinder::getDimensions() const
00285 {
00286   std::vector<double> d(2);
00287   d[0] = radius_;
00288   d[1] = length_;
00289   return d;
00290 }
00291 
00292 void bodies::Cylinder::updateInternalData()
00293 {
00294   radiusU_ = radius_ * scale_ + padding_;
00295   radius2_ = radiusU_ * radiusU_;
00296   length2_ = scale_ * length_ / 2.0 + padding_;
00297   center_ = pose_.translation();
00298   radiusBSqr_ = length2_ * length2_ + radius2_;
00299   radiusB_ = sqrt(radiusBSqr_);
00300 
00301   Eigen::Matrix3d basis = pose_.rotation();
00302   normalB1_ = basis.col(0);
00303   normalB2_ = basis.col(1);
00304   normalH_  = basis.col(2);
00305 
00306   double tmp = -normalH_.dot(center_);
00307   d1_ = tmp + length2_;
00308   d2_ = tmp - length2_;
00309 }
00310 
00311 bool bodies::Cylinder::samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result)
00312 {
00313   // sample a point on the base disc of the cylinder
00314   double a = rng.uniformReal(-boost::math::constants::pi<double>(), boost::math::constants::pi<double>());
00315   double r = rng.uniformReal(-radiusU_, radiusU_);
00316   double x = cos(a) * r;
00317   double y = sin(a) * r;
00318 
00319   // sample e height
00320   double z = rng.uniformReal(-length2_, length2_);
00321 
00322   result = Eigen::Vector3d(x, y, z);
00323   return true;
00324 }
00325 
00326 boost::shared_ptr<bodies::Body> bodies::Cylinder::cloneAt(const Eigen::Affine3d &pose, double padding, double scale) const
00327 {
00328   Cylinder *c = new Cylinder();
00329   c->length_ = length_;
00330   c->radius_ = radius_;
00331   c->padding_ = padding;
00332   c->scale_ = scale;
00333   c->pose_ = pose;
00334   c->updateInternalData();
00335   return boost::shared_ptr<Body>(c);
00336 }
00337 
00338 double bodies::Cylinder::computeVolume() const
00339 {
00340   return 2.0 * boost::math::constants::pi<double>() * radius2_ * length2_;
00341 }
00342 
00343 void bodies::Cylinder::computeBoundingSphere(BoundingSphere &sphere) const
00344 {
00345   sphere.center = center_;
00346   sphere.radius = radiusB_;
00347 }
00348 
00349 void bodies::Cylinder::computeBoundingCylinder(BoundingCylinder &cylinder) const
00350 {
00351   cylinder.pose = pose_;
00352   cylinder.radius = radiusU_;
00353   cylinder.length = scale_*length_+padding_;
00354 }
00355 
00356 bool bodies::Cylinder::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d *intersections, unsigned int count) const
00357 {
00358   if (detail::distanceSQR(center_, origin, dir) > radiusBSqr_) return false;
00359 
00360   std::vector<detail::intersc> ipts;
00361 
00362   // intersect bases
00363   double tmp = normalH_.dot(dir);
00364   if (fabs(tmp) > detail::ZERO)
00365   {
00366     double tmp2 = -normalH_.dot(origin);
00367     double t1 = (tmp2 - d1_) / tmp;
00368 
00369     if (t1 > 0.0)
00370     {
00371       Eigen::Vector3d p1(origin + dir * t1);
00372       Eigen::Vector3d v1(p1 - center_);
00373       v1 = v1 - normalH_.dot(v1) * normalH_;
00374       if (v1.squaredNorm() < radius2_ + detail::ZERO)
00375       {
00376         if (intersections == NULL)
00377           return true;
00378 
00379         detail::intersc ip(p1, t1);
00380         ipts.push_back(ip);
00381       }
00382     }
00383 
00384     double t2 = (tmp2 - d2_) / tmp;
00385     if (t2 > 0.0)
00386     {
00387       Eigen::Vector3d p2(origin + dir * t2);
00388       Eigen::Vector3d v2(p2 - center_);
00389       v2 = v2 - normalH_.dot(v2) * normalH_;
00390       if (v2.squaredNorm() < radius2_ + detail::ZERO)
00391       {
00392         if (intersections == NULL)
00393           return true;
00394 
00395         detail::intersc ip(p2, t2);
00396         ipts.push_back(ip);
00397       }
00398     }
00399   }
00400 
00401   if (ipts.size() < 2)
00402   {
00403     // intersect with infinite cylinder
00404     Eigen::Vector3d VD(normalH_.cross(dir));
00405     Eigen::Vector3d ROD(normalH_.cross(origin - center_));
00406     double a = VD.squaredNorm();
00407     double b = 2.0 * ROD.dot(VD);
00408     double c = ROD.squaredNorm() - radius2_;
00409     double d = b * b - 4.0 * a * c;
00410     if (d > 0.0 && fabs(a) > detail::ZERO)
00411     {
00412       d = sqrt(d);
00413       double e = -a * 2.0;
00414       double t1 = (b + d) / e;
00415       double t2 = (b - d) / e;
00416 
00417       if (t1 > 0.0)
00418       {
00419         Eigen::Vector3d p1(origin + dir * t1);
00420         Eigen::Vector3d v1(center_ - p1);
00421 
00422         if (fabs(normalH_.dot(v1)) < length2_ + detail::ZERO)
00423         {
00424           if (intersections == NULL)
00425             return true;
00426 
00427           detail::intersc ip(p1, t1);
00428           ipts.push_back(ip);
00429         }
00430       }
00431 
00432       if (t2 > 0.0)
00433       {
00434         Eigen::Vector3d p2(origin + dir * t2);
00435         Eigen::Vector3d v2(center_ - p2);
00436 
00437         if (fabs(normalH_.dot(v2)) < length2_ + detail::ZERO)
00438         {
00439           if (intersections == NULL)
00440             return true;
00441           detail::intersc ip(p2, t2);
00442           ipts.push_back(ip);
00443         }
00444       }
00445     }
00446   }
00447 
00448   if (ipts.empty())
00449     return false;
00450 
00451   std::sort(ipts.begin(), ipts.end(), detail::interscOrder());
00452   const unsigned int n = count > 0 ? std::min<unsigned int>(count, ipts.size()) : ipts.size();
00453   for (unsigned int i = 0 ; i < n ; ++i)
00454     intersections->push_back(ipts[i].pt);
00455 
00456   return true;
00457 }
00458 
00459 bool bodies::Box::samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int /* max_attempts */, Eigen::Vector3d &result)
00460 {
00461   result = pose_ * Eigen::Vector3d(rng.uniformReal(-length2_, length2_),
00462                                    rng.uniformReal(-width2_, width2_),
00463                                    rng.uniformReal(-height2_, height2_));
00464   return true;
00465 }
00466 
00467 bool bodies::Box::containsPoint(const Eigen::Vector3d &p, bool verbose) const
00468 {
00469   Eigen::Vector3d v = p - center_;
00470   double pL = v.dot(normalL_);
00471   if (fabs(pL) > length2_)
00472     return false;
00473 
00474   double pW = v.dot(normalW_);
00475   if (fabs(pW) > width2_)
00476     return false;
00477 
00478   double pH = v.dot(normalH_);
00479   if (fabs(pH) > height2_)
00480     return false;
00481 
00482   return true;
00483 }
00484 
00485 void bodies::Box::useDimensions(const shapes::Shape *shape) // (x, y, z) = (length, width, height)
00486 {
00487   const double *size = static_cast<const shapes::Box*>(shape)->size;
00488   length_ = size[0];
00489   width_  = size[1];
00490   height_ = size[2];
00491 }
00492 
00493 std::vector<double> bodies::Box::getDimensions() const
00494 {
00495   std::vector<double> d(3);
00496   d[0] = length_;
00497   d[1] = width_;
00498   d[2] = height_;
00499   return d;
00500 }
00501 
00502 void bodies::Box::updateInternalData()
00503 {
00504   double s2 = scale_ / 2.0;
00505   length2_ = length_ * s2 + padding_;
00506   width2_  = width_ * s2 + padding_;
00507   height2_ = height_ * s2 + padding_;
00508 
00509   center_  = pose_.translation();
00510 
00511   radius2_ = length2_ * length2_ + width2_ * width2_ + height2_ * height2_;
00512   radiusB_ = sqrt(radius2_);
00513 
00514   Eigen::Matrix3d basis = pose_.rotation();
00515   normalL_ = basis.col(0);
00516   normalW_ = basis.col(1);
00517   normalH_ = basis.col(2);
00518 
00519   const Eigen::Vector3d tmp(normalL_ * length2_ + normalW_ * width2_ + normalH_ * height2_);
00520   corner1_ = center_ - tmp;
00521   corner2_ = center_ + tmp;
00522 }
00523 
00524 boost::shared_ptr<bodies::Body> bodies::Box::cloneAt(const Eigen::Affine3d &pose, double padding, double scale) const
00525 {
00526   Box *b = new Box();
00527   b->length_ = length_;
00528   b->width_ = width_;
00529   b->height_ = height_;
00530   b->padding_ = padding;
00531   b->scale_ = scale;
00532   b->pose_ = pose;
00533   b->updateInternalData();
00534   return boost::shared_ptr<Body>(b);
00535 }
00536 
00537 double bodies::Box::computeVolume() const
00538 {
00539   return 8.0 * length2_ * width2_ * height2_;
00540 }
00541 
00542 void bodies::Box::computeBoundingSphere(BoundingSphere &sphere) const
00543 {
00544   sphere.center = center_;
00545   sphere.radius = radiusB_;
00546 }
00547 
00548 void bodies::Box::computeBoundingCylinder(BoundingCylinder &cylinder) const
00549 {
00550   double a, b;
00551 
00552   if(length2_ > width2_ && length2_ > height2_)
00553   {
00554     cylinder.length = length2_*2.0;
00555     a = width2_;
00556     b = height2_;
00557     Eigen::Affine3d rot(Eigen::AngleAxisd(90.0f * (M_PI/180.0f), Eigen::Vector3d::UnitY()));
00558     cylinder.pose = pose_*rot;
00559   }
00560   else
00561     if(width2_ > height2_)
00562     {
00563       cylinder.length = width2_*2.0;
00564       a = height2_;
00565       b = length2_;
00566       cylinder.radius = sqrt(height2_*height2_+length2_*length2_);
00567       Eigen::Affine3d rot(Eigen::AngleAxisd(90.0f * (M_PI/180.0f), Eigen::Vector3d::UnitX()));
00568       cylinder.pose = pose_*rot;
00569     }
00570     else
00571     {
00572       cylinder.length = height2_ * 2.0;
00573       a = width2_;
00574       b = length2_;
00575       cylinder.pose = pose_;
00576     }
00577   cylinder.radius = sqrt(a * a + b * b);
00578 }
00579 
00580 bool bodies::Box::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d *intersections, unsigned int count) const
00581 {
00582   if (detail::distanceSQR(center_, origin, dir) > radius2_) return false;
00583 
00584   double t_near = -std::numeric_limits<double>::infinity();
00585   double t_far  = std::numeric_limits<double>::infinity();
00586 
00587   for (int i = 0; i < 3; i++)
00588   {
00589     const Eigen::Vector3d &vN = i == 0 ? normalL_ : (i == 1 ? normalW_ : normalH_);
00590     double dp = vN.dot(dir);
00591 
00592     if (fabs(dp) > detail::ZERO)
00593     {
00594       double t1 = vN.dot(corner1_ - origin) / dp;
00595       double t2 = vN.dot(corner2_ - origin) / dp;
00596 
00597       if (t1 > t2)
00598         std::swap(t1, t2);
00599 
00600       if (t1 > t_near)
00601         t_near = t1;
00602 
00603       if (t2 < t_far)
00604         t_far = t2;
00605 
00606       if (t_near > t_far)
00607         return false;
00608 
00609       if (t_far < 0.0)
00610         return false;
00611     }
00612     else
00613     {
00614       if (i == 0)
00615       {
00616         if ((std::min(corner1_.y(), corner2_.y()) > origin.y() ||
00617              std::max(corner1_.y(), corner2_.y()) < origin.y()) &&
00618             (std::min(corner1_.z(), corner2_.z()) > origin.z() ||
00619              std::max(corner1_.z(), corner2_.z()) < origin.z()))
00620           return false;
00621       }
00622       else
00623       {
00624         if (i == 1)
00625         {
00626           if ((std::min(corner1_.x(), corner2_.x()) > origin.x() ||
00627                std::max(corner1_.x(), corner2_.x()) < origin.x()) &&
00628               (std::min(corner1_.z(), corner2_.z()) > origin.z() ||
00629                std::max(corner1_.z(), corner2_.z()) < origin.z()))
00630             return false;
00631         }
00632         else
00633           if ((std::min(corner1_.x(), corner2_.x()) > origin.x() ||
00634                std::max(corner1_.x(), corner2_.x()) < origin.x()) &&
00635               (std::min(corner1_.y(), corner2_.y()) > origin.y() ||
00636                std::max(corner1_.y(), corner2_.y()) < origin.y()))
00637             return false;
00638       }
00639     }
00640   }
00641 
00642   if (intersections)
00643   {
00644     if (t_far - t_near > detail::ZERO)
00645     {
00646       intersections->push_back(t_near * dir + origin);
00647       if (count > 1)
00648         intersections->push_back(t_far  * dir + origin);
00649     }
00650     else
00651       intersections->push_back(t_far * dir + origin);
00652   }
00653 
00654   return true;
00655 }
00656 
00657 bool bodies::ConvexMesh::containsPoint(const Eigen::Vector3d &p, bool verbose) const
00658 {
00659   if (!mesh_data_) return false;
00660   if (bounding_box_.containsPoint(p))
00661   {
00662     Eigen::Vector3d ip(i_pose_ * p);
00663     ip = mesh_data_->mesh_center_ + (ip - mesh_data_->mesh_center_) * scale_;
00664     return isPointInsidePlanes(ip);
00665   }
00666   else
00667     return false;
00668 }
00669 
00670 void bodies::ConvexMesh::useDimensions(const shapes::Shape *shape)
00671 {
00672   mesh_data_.reset(new MeshData());
00673   const shapes::Mesh *mesh = static_cast<const shapes::Mesh*>(shape);
00674 
00675   double maxX = -std::numeric_limits<double>::infinity(), maxY = -std::numeric_limits<double>::infinity(), maxZ = -std::numeric_limits<double>::infinity();
00676   double minX =  std::numeric_limits<double>::infinity(), minY =  std::numeric_limits<double>::infinity(), minZ  = std::numeric_limits<double>::infinity();
00677 
00678   for(unsigned int i = 0; i < mesh->vertex_count ; ++i)
00679   {
00680     double vx = mesh->vertices[3 * i    ];
00681     double vy = mesh->vertices[3 * i + 1];
00682     double vz = mesh->vertices[3 * i + 2];
00683 
00684     if (maxX < vx) maxX = vx;
00685     if (maxY < vy) maxY = vy;
00686     if (maxZ < vz) maxZ = vz;
00687 
00688     if (minX > vx) minX = vx;
00689     if (minY > vy) minY = vy;
00690     if (minZ > vz) minZ = vz;
00691   }
00692 
00693   if (maxX < minX) maxX = minX = 0.0;
00694   if (maxY < minY) maxY = minY = 0.0;
00695   if (maxZ < minZ) maxZ = minZ = 0.0;
00696 
00697   mesh_data_->box_size_ = Eigen::Vector3d(maxX - minX, maxY - minY, maxZ - minZ);
00698 
00699   mesh_data_->box_offset_ = Eigen::Vector3d((minX + maxX) / 2.0, (minY + maxY) / 2.0, (minZ + maxZ) / 2.0);
00700 
00701   mesh_data_->planes_.clear();
00702   mesh_data_->triangles_.clear();
00703   mesh_data_->vertices_.clear();
00704   mesh_data_->mesh_radiusB_ = 0.0;
00705   mesh_data_->mesh_center_ = Eigen::Vector3d();
00706 
00707   double xdim = maxX - minX;
00708   double ydim = maxY - minY;
00709   double zdim = maxZ - minZ;
00710 
00711   double pose1;
00712   double pose2;
00713 
00714   unsigned int off1;
00715   unsigned int off2;
00716 
00717   /* compute bounding cylinder */
00718   double cyl_length;
00719   double maxdist = -std::numeric_limits<double>::infinity();
00720   if (xdim > ydim && xdim > zdim)
00721   {
00722     off1 = 1;
00723     off2 = 2;
00724     pose1 = mesh_data_->box_offset_.y();
00725     pose2 = mesh_data_->box_offset_.z();
00726     cyl_length = xdim;
00727   }
00728   else if(ydim > zdim)
00729   {
00730     off1 = 0;
00731     off2 = 2;
00732     pose1 = mesh_data_->box_offset_.x();
00733     pose2 = mesh_data_->box_offset_.z();
00734     cyl_length = ydim;
00735   }
00736   else
00737   {
00738     off1 = 0;
00739     off2 = 1;
00740     pose1 = mesh_data_->box_offset_.x();
00741     pose2 = mesh_data_->box_offset_.y();
00742     cyl_length = zdim;
00743   }
00744 
00745   /* compute convex hull */
00746   coordT *points = (coordT *)calloc(mesh->vertex_count*3, sizeof(coordT));
00747   for(unsigned int i = 0; i < mesh->vertex_count ; ++i)
00748   {
00749     points[3*i+0] = (coordT) mesh->vertices[3*i+0];
00750     points[3*i+1] = (coordT) mesh->vertices[3*i+1];
00751     points[3*i+2] = (coordT) mesh->vertices[3*i+2];
00752 
00753     double dista = mesh->vertices[3 * i + off1]-pose1;
00754     double distb = mesh->vertices[3 * i + off2]-pose2;
00755     double dist = sqrt(((dista*dista)+(distb*distb)));
00756     if(dist > maxdist)
00757       maxdist = dist;
00758   }
00759   mesh_data_->bounding_cylinder_.radius = maxdist;
00760   mesh_data_->bounding_cylinder_.length = cyl_length;
00761 
00762   FILE* null = fopen ("/dev/null","w");
00763 
00764   char flags[] = "qhull Tv";
00765   int exitcode = qh_new_qhull(3, mesh->vertex_count, points, true, flags, null, null);
00766 
00767   if (exitcode != 0)
00768   {
00769     logWarn("Convex hull creation failed");
00770     qh_freeqhull (!qh_ALL);
00771     int curlong, totlong;
00772     qh_memfreeshort (&curlong, &totlong);
00773     return;
00774   }
00775 
00776   int num_facets = qh num_facets;
00777 
00778   int num_vertices = qh num_vertices;
00779   mesh_data_->vertices_.reserve(num_vertices);
00780   Eigen::Vector3d sum(0, 0, 0);
00781 
00782   //necessary for FORALLvertices
00783   std::map<unsigned int, unsigned int> qhull_vertex_table;
00784   vertexT * vertex;
00785   FORALLvertices
00786   {
00787     Eigen::Vector3d vert(vertex->point[0],
00788                          vertex->point[1],
00789                          vertex->point[2]);
00790     qhull_vertex_table[vertex->id] = mesh_data_->vertices_.size();
00791     sum += vert;
00792     mesh_data_->vertices_.push_back(vert);
00793   }
00794 
00795   mesh_data_->mesh_center_ = sum / (double)(num_vertices);
00796   for (unsigned int j = 0 ; j < mesh_data_->vertices_.size() ; ++j)
00797   {
00798     double dist = (mesh_data_->vertices_[j] - mesh_data_->mesh_center_).squaredNorm();
00799     if (dist > mesh_data_->mesh_radiusB_)
00800       mesh_data_->mesh_radiusB_ = dist;
00801   }
00802 
00803   mesh_data_->mesh_radiusB_ = sqrt(mesh_data_->mesh_radiusB_);
00804   mesh_data_->triangles_.reserve(num_facets);
00805 
00806   //neccessary for qhull macro
00807   facetT * facet;
00808   FORALLfacets
00809   {
00810     Eigen::Vector4f planeEquation(facet->normal[0], facet->normal[1], facet->normal[2], facet->offset);
00811     mesh_data_->planes_.push_back(planeEquation);
00812 
00813     // Needed by FOREACHvertex_i_
00814     int vertex_n, vertex_i;
00815     FOREACHvertex_i_ ((*facet).vertices)
00816     {
00817       mesh_data_->triangles_.push_back(qhull_vertex_table[vertex->id]);
00818     }
00819 
00820   }
00821   qh_freeqhull(!qh_ALL);
00822   int curlong, totlong;
00823   qh_memfreeshort (&curlong, &totlong);
00824 }
00825 
00826 std::vector<double> bodies::ConvexMesh::getDimensions() const
00827 {
00828   return std::vector<double>();
00829 }
00830 
00831 void bodies::ConvexMesh::updateInternalData()
00832 {
00833   if (!mesh_data_)
00834     return;
00835   Eigen::Affine3d pose = pose_;
00836   pose.translation() = Eigen::Vector3d(pose_ * mesh_data_->box_offset_);
00837 
00838   boost::scoped_ptr<shapes::Box> box_shape(new shapes::Box(mesh_data_->box_size_.x(), mesh_data_->box_size_.y(), mesh_data_->box_size_.z()));
00839   bounding_box_.setDimensions(box_shape.get());
00840   bounding_box_.setPose(pose);
00841   bounding_box_.setPadding(padding_);
00842   bounding_box_.setScale(scale_);
00843 
00844   i_pose_ = pose_.inverse();
00845   center_ = pose_ * mesh_data_->mesh_center_;
00846   radiusB_ = mesh_data_->mesh_radiusB_ * scale_ + padding_;
00847   radiusBSqr_ = radiusB_ * radiusB_;
00848 
00849   // compute the scaled vertices, if needed
00850   if (padding_ == 0.0 && scale_ == 1.0)
00851     scaled_vertices_ = &mesh_data_->vertices_;
00852   else
00853   {
00854     if (!scaled_vertices_storage_)
00855       scaled_vertices_storage_.reset(new EigenSTL::vector_Vector3d());
00856     scaled_vertices_ = scaled_vertices_storage_.get();
00857     scaled_vertices_storage_->resize(mesh_data_->vertices_.size());
00858     for (unsigned int i = 0 ; i < mesh_data_->vertices_.size() ; ++i)
00859     {
00860       Eigen::Vector3d v(mesh_data_->vertices_[i] - mesh_data_->mesh_center_);
00861       double l = v.norm();
00862       scaled_vertices_storage_->at(i) = mesh_data_->mesh_center_ + v * (scale_ + (l > detail::ZERO ? padding_ / l : 0.0));
00863     }
00864   }
00865 }
00866 const std::vector<unsigned int>& bodies::ConvexMesh::getTriangles() const
00867 {
00868   static const std::vector<unsigned int> empty;
00869   return mesh_data_ ? mesh_data_->triangles_ : empty;
00870 }
00871 
00872 const EigenSTL::vector_Vector3d& bodies::ConvexMesh::getVertices() const
00873 {
00874   static const EigenSTL::vector_Vector3d empty;
00875   return mesh_data_ ? mesh_data_->vertices_ : empty;
00876 }
00877 
00878 const EigenSTL::vector_Vector3d& bodies::ConvexMesh::getScaledVertices() const
00879 {
00880   return scaled_vertices_ ? *scaled_vertices_ : getVertices();
00881 }
00882 
00883 boost::shared_ptr<bodies::Body> bodies::ConvexMesh::cloneAt(const Eigen::Affine3d &pose, double padding, double scale) const
00884 {
00885   ConvexMesh *m = new ConvexMesh();
00886   m->mesh_data_ = mesh_data_;
00887   m->padding_ = padding;
00888   m->scale_ = scale;
00889   m->pose_ = pose;
00890   m->updateInternalData();
00891   return boost::shared_ptr<Body>(m);
00892 }
00893 
00894 void bodies::ConvexMesh::computeBoundingSphere(BoundingSphere &sphere) const
00895 {
00896   sphere.center = center_;
00897   sphere.radius = radiusB_;
00898 }
00899 
00900 void bodies::ConvexMesh::computeBoundingCylinder(BoundingCylinder &cylinder) const
00901 {
00902   cylinder.length = mesh_data_ ? mesh_data_->bounding_cylinder_.length : 0.0;
00903   cylinder.radius = mesh_data_ ? mesh_data_->bounding_cylinder_.radius : 0.0;
00904   //need to do rotation correctly to get pose, which bounding box does
00905   BoundingCylinder cyl;
00906   bounding_box_.computeBoundingCylinder(cyl);
00907   cylinder.pose = cyl.pose;
00908 }
00909 
00910 bool bodies::ConvexMesh::isPointInsidePlanes(const Eigen::Vector3d& point) const
00911 {
00912   unsigned int numplanes = mesh_data_->planes_.size();
00913   for (unsigned int i = 0 ; i < numplanes ; ++i)
00914   {
00915     const Eigen::Vector4f& plane = mesh_data_->planes_[i];
00916     Eigen::Vector3d plane_vec(plane.x(), plane.y(), plane.z());
00917     double dist = plane_vec.dot(point) + plane.w() - padding_ - 1e-6;
00918     if (dist > 0.0)
00919       return false;
00920   }
00921   return true;
00922 }
00923 
00924 unsigned int bodies::ConvexMesh::countVerticesBehindPlane(const Eigen::Vector4f& planeNormal) const
00925 {
00926   unsigned int numvertices = mesh_data_->vertices_.size();
00927   unsigned int result = 0;
00928   for (unsigned int i = 0 ; i < numvertices ; ++i)
00929   {
00930     Eigen::Vector3d plane_vec(planeNormal.x(), planeNormal.y(), planeNormal.z());
00931     double dist = plane_vec.dot(mesh_data_->vertices_[i]) + planeNormal.w() - 1e-6;
00932     if (dist > 0.0)
00933       result++;
00934   }
00935   return result;
00936 }
00937 
00938 double bodies::ConvexMesh::computeVolume() const
00939 {
00940   double volume = 0.0;
00941   if (mesh_data_)
00942     for (unsigned int i = 0 ; i < mesh_data_->triangles_.size() / 3 ; ++i)
00943     {
00944       const Eigen::Vector3d &v1 = mesh_data_->vertices_[mesh_data_->triangles_[3*i + 0]];
00945       const Eigen::Vector3d &v2 = mesh_data_->vertices_[mesh_data_->triangles_[3*i + 1]];
00946       const Eigen::Vector3d &v3 = mesh_data_->vertices_[mesh_data_->triangles_[3*i + 2]];
00947       volume += v1.x()*v2.y()*v3.z() + v2.x()*v3.y()*v1.z() + v3.x()*v1.y()*v2.z() - v1.x()*v3.y()*v2.z() - v2.x()*v1.y()*v3.z() - v3.x()*v2.y()*v1.z();
00948     }
00949   return fabs(volume)/6.0;
00950 }
00951 
00952 bool bodies::ConvexMesh::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d *intersections, unsigned int count) const
00953 {
00954   if (!mesh_data_) return false;
00955   if (detail::distanceSQR(center_, origin, dir) > radiusBSqr_) return false;
00956   if (!bounding_box_.intersectsRay(origin, dir)) return false;
00957 
00958   // transform the ray into the coordinate frame of the mesh
00959   Eigen::Vector3d orig(i_pose_ * origin);
00960   Eigen::Vector3d dr(i_pose_ * dir);
00961 
00962   std::vector<detail::intersc> ipts;
00963 
00964   bool result = false;
00965 
00966   // for each triangle
00967   const unsigned int nt = mesh_data_->triangles_.size() / 3;
00968   for (unsigned int i = 0 ; i < nt ; ++i)
00969   {
00970     Eigen::Vector3d vec(mesh_data_->planes_[i].x(),
00971                         mesh_data_->planes_[i].y(),
00972                         mesh_data_->planes_[i].z());
00973 
00974     double tmp = vec.dot(dr);
00975     if (fabs(tmp) > detail::ZERO)
00976     {
00977 
00978       double t = -(vec.dot(orig) + mesh_data_->planes_[i].w()) / tmp;
00979       if (t > 0.0)
00980       {
00981         const int i3 = 3 * i;
00982         const int v1 = mesh_data_->triangles_[i3 + 0];
00983         const int v2 = mesh_data_->triangles_[i3 + 1];
00984         const int v3 = mesh_data_->triangles_[i3 + 2];
00985 
00986         const Eigen::Vector3d &a = scaled_vertices_->at(v1);
00987         const Eigen::Vector3d &b = scaled_vertices_->at(v2);
00988         const Eigen::Vector3d &c = scaled_vertices_->at(v3);
00989 
00990         Eigen::Vector3d cb(c - b);
00991         Eigen::Vector3d ab(a - b);
00992 
00993         // intersection of the plane defined by the triangle and the ray
00994         Eigen::Vector3d P(orig + dr * t);
00995 
00996         // check if it is inside the triangle
00997         Eigen::Vector3d pb(P - b);
00998         Eigen::Vector3d c1(cb.cross(pb));
00999         Eigen::Vector3d c2(cb.cross(ab));
01000         if (c1.dot(c2) < 0.0)
01001           continue;
01002 
01003         Eigen::Vector3d ca(c - a);
01004         Eigen::Vector3d pa(P - a);
01005         Eigen::Vector3d ba(-ab);
01006 
01007         c1 = ca.cross(pa);
01008         c2 = ca.cross(ba);
01009         if (c1.dot(c2) < 0.0)
01010           continue;
01011 
01012         c1 = ba.cross(pa);
01013         c2 = ba.cross(ca);
01014 
01015         if (c1.dot(c2) < 0.0)
01016           continue;
01017 
01018         result = true;
01019         if (intersections)
01020         {
01021           detail::intersc ip(origin + dir * t, t);
01022           ipts.push_back(ip);
01023         }
01024         else
01025           break;
01026       }
01027     }
01028   }
01029 
01030   if (intersections)
01031   {
01032     std::sort(ipts.begin(), ipts.end(), detail::interscOrder());
01033     const unsigned int n = count > 0 ? std::min<unsigned int>(count, ipts.size()) : ipts.size();
01034     for (unsigned int i = 0 ; i < n ; ++i)
01035       intersections->push_back(ipts[i].pt);
01036   }
01037 
01038   return result;
01039 }
01040 
01041 bodies::BodyVector::BodyVector()
01042 {
01043 }
01044 
01045 bodies::BodyVector::BodyVector(const std::vector<shapes::Shape*>& shapes,
01046                                const EigenSTL::vector_Affine3d& poses,
01047                                double padding)
01048 {
01049   for(unsigned int i = 0; i < shapes.size(); i++)
01050     addBody(shapes[i], poses[i], padding);
01051 }
01052 
01053 
01054 bodies::BodyVector::~BodyVector()
01055 {
01056   clear();
01057 }
01058 
01059 void bodies::BodyVector::clear()
01060 {
01061   for(unsigned int i = 0; i < bodies_.size(); i++)
01062     delete bodies_[i];
01063   bodies_.clear();
01064 }
01065 
01066 void bodies::BodyVector::addBody(Body *body)
01067 {
01068   bodies_.push_back(body);
01069   BoundingSphere sphere;
01070   body->computeBoundingSphere(sphere);
01071 }
01072 
01073 void bodies::BodyVector::addBody(const shapes::Shape *shape, const Eigen::Affine3d& pose, double padding)
01074 {
01075   bodies::Body* body = bodies::createBodyFromShape(shape);
01076   body->setPose(pose);
01077   body->setPadding(padding);
01078   addBody(body);
01079 }
01080 
01081 std::size_t bodies::BodyVector::getCount() const
01082 {
01083   return bodies_.size();
01084 }
01085 
01086 void bodies::BodyVector::setPose(unsigned int i, const Eigen::Affine3d& pose)
01087 {
01088   if (i >= bodies_.size())
01089   {
01090     logError("There is no body at index %u", i);
01091     return;
01092   }
01093 
01094   bodies_[i]->setPose(pose);
01095 }
01096 
01097 const bodies::Body* bodies::BodyVector::getBody(unsigned int i) const
01098 {
01099   if (i >= bodies_.size())
01100   {
01101     logError("There is no body at index %u", i);
01102     return NULL;
01103   }
01104   else
01105     return bodies_[i];
01106 }
01107 
01108 bool bodies::BodyVector::containsPoint(const Eigen::Vector3d &p, std::size_t &index, bool verbose) const
01109 {
01110   for (std::size_t i = 0 ; i < bodies_.size() ; ++i)
01111     if (bodies_[i]->containsPoint(p, verbose))
01112     {
01113       index = i;
01114       return true;
01115     }
01116   return false;
01117 }
01118 
01119 bool bodies::BodyVector::containsPoint(const Eigen::Vector3d &p, bool verbose) const
01120 {
01121   std::size_t dummy;
01122   return containsPoint(p, dummy, verbose);
01123 }
01124 
01125 bool bodies::BodyVector::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d &dir, std::size_t &index, EigenSTL::vector_Vector3d *intersections, unsigned int count) const
01126 {
01127   for (std::size_t i = 0 ; i < bodies_.size() ; ++i)
01128     if (bodies_[i]->intersectsRay(origin, dir, intersections, count))
01129     {
01130       index = i;
01131       return true;
01132     }
01133   return false;
01134 }


geometric_shapes
Author(s): Ioan Sucan
autogenerated on Mon Oct 6 2014 00:11:19