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::correctVertexOrderFromPlanes()
00671 {
00672     for(unsigned int i = 0; i < mesh_data_->triangles_.size(); i += 3) {
00673         Eigen::Vector3d d1 = mesh_data_->vertices_[mesh_data_->triangles_[i]]
00674             - mesh_data_->vertices_[mesh_data_->triangles_[i + 1]];
00675         Eigen::Vector3d d2 = mesh_data_->vertices_[mesh_data_->triangles_[i]]
00676             - mesh_data_->vertices_[mesh_data_->triangles_[i + 2]];
00677         // expected computed normal from triangle vertex order
00678         Eigen::Vector3d tri_normal = d1.cross(d2);
00679         tri_normal.normalize();
00680         // actual plane normal
00681         Eigen::Vector3d normal(
00682                 mesh_data_->planes_[mesh_data_->plane_for_triangle_[i/3]].x(),
00683                 mesh_data_->planes_[mesh_data_->plane_for_triangle_[i/3]].y(),
00684                 mesh_data_->planes_[mesh_data_->plane_for_triangle_[i/3]].z());
00685         bool same_dir = tri_normal.dot(normal) > 0;
00686         if(!same_dir) {
00687             std::swap(mesh_data_->triangles_[i], mesh_data_->triangles_[i + 1]);
00688         }
00689     }
00690 }
00691 
00692 void bodies::ConvexMesh::useDimensions(const shapes::Shape *shape)
00693 {
00694   mesh_data_.reset(new MeshData());
00695   const shapes::Mesh *mesh = static_cast<const shapes::Mesh*>(shape);
00696 
00697   double maxX = -std::numeric_limits<double>::infinity(), maxY = -std::numeric_limits<double>::infinity(), maxZ = -std::numeric_limits<double>::infinity();
00698   double minX =  std::numeric_limits<double>::infinity(), minY =  std::numeric_limits<double>::infinity(), minZ  = std::numeric_limits<double>::infinity();
00699 
00700   for(unsigned int i = 0; i < mesh->vertex_count ; ++i)
00701   {
00702     double vx = mesh->vertices[3 * i    ];
00703     double vy = mesh->vertices[3 * i + 1];
00704     double vz = mesh->vertices[3 * i + 2];
00705 
00706     if (maxX < vx) maxX = vx;
00707     if (maxY < vy) maxY = vy;
00708     if (maxZ < vz) maxZ = vz;
00709 
00710     if (minX > vx) minX = vx;
00711     if (minY > vy) minY = vy;
00712     if (minZ > vz) minZ = vz;
00713   }
00714 
00715   if (maxX < minX) maxX = minX = 0.0;
00716   if (maxY < minY) maxY = minY = 0.0;
00717   if (maxZ < minZ) maxZ = minZ = 0.0;
00718 
00719   mesh_data_->box_size_ = Eigen::Vector3d(maxX - minX, maxY - minY, maxZ - minZ);
00720 
00721   mesh_data_->box_offset_ = Eigen::Vector3d((minX + maxX) / 2.0, (minY + maxY) / 2.0, (minZ + maxZ) / 2.0);
00722 
00723   mesh_data_->planes_.clear();
00724   mesh_data_->triangles_.clear();
00725   mesh_data_->vertices_.clear();
00726   mesh_data_->mesh_radiusB_ = 0.0;
00727   mesh_data_->mesh_center_ = Eigen::Vector3d();
00728 
00729   double xdim = maxX - minX;
00730   double ydim = maxY - minY;
00731   double zdim = maxZ - minZ;
00732 
00733   double pose1;
00734   double pose2;
00735 
00736   unsigned int off1;
00737   unsigned int off2;
00738 
00739   /* compute bounding cylinder */
00740   double cyl_length;
00741   double maxdist = -std::numeric_limits<double>::infinity();
00742   if (xdim > ydim && xdim > zdim)
00743   {
00744     off1 = 1;
00745     off2 = 2;
00746     pose1 = mesh_data_->box_offset_.y();
00747     pose2 = mesh_data_->box_offset_.z();
00748     cyl_length = xdim;
00749   }
00750   else if(ydim > zdim)
00751   {
00752     off1 = 0;
00753     off2 = 2;
00754     pose1 = mesh_data_->box_offset_.x();
00755     pose2 = mesh_data_->box_offset_.z();
00756     cyl_length = ydim;
00757   }
00758   else
00759   {
00760     off1 = 0;
00761     off2 = 1;
00762     pose1 = mesh_data_->box_offset_.x();
00763     pose2 = mesh_data_->box_offset_.y();
00764     cyl_length = zdim;
00765   }
00766 
00767   /* compute convex hull */
00768   coordT *points = (coordT *)calloc(mesh->vertex_count*3, sizeof(coordT));
00769   for(unsigned int i = 0; i < mesh->vertex_count ; ++i)
00770   {
00771     points[3*i+0] = (coordT) mesh->vertices[3*i+0];
00772     points[3*i+1] = (coordT) mesh->vertices[3*i+1];
00773     points[3*i+2] = (coordT) mesh->vertices[3*i+2];
00774 
00775     double dista = mesh->vertices[3 * i + off1]-pose1;
00776     double distb = mesh->vertices[3 * i + off2]-pose2;
00777     double dist = sqrt(((dista*dista)+(distb*distb)));
00778     if(dist > maxdist)
00779       maxdist = dist;
00780   }
00781   mesh_data_->bounding_cylinder_.radius = maxdist;
00782   mesh_data_->bounding_cylinder_.length = cyl_length;
00783 
00784   static FILE* null = fopen ("/dev/null","w");
00785 
00786   char flags[] = "qhull Tv Qt";
00787   int exitcode = qh_new_qhull(3, mesh->vertex_count, points, true, flags, null, null);
00788 
00789   if (exitcode != 0)
00790   {
00791     logWarn("Convex hull creation failed");
00792     qh_freeqhull (!qh_ALL);
00793     int curlong, totlong;
00794     qh_memfreeshort (&curlong, &totlong);
00795     return;
00796   }
00797 
00798   int num_facets = qh num_facets;
00799 
00800   int num_vertices = qh num_vertices;
00801   mesh_data_->vertices_.reserve(num_vertices);
00802   Eigen::Vector3d sum(0, 0, 0);
00803 
00804   //necessary for FORALLvertices
00805   std::map<unsigned int, unsigned int> qhull_vertex_table;
00806   vertexT * vertex;
00807   FORALLvertices
00808   {
00809     Eigen::Vector3d vert(vertex->point[0],
00810                          vertex->point[1],
00811                          vertex->point[2]);
00812     qhull_vertex_table[vertex->id] = mesh_data_->vertices_.size();
00813     sum += vert;
00814     mesh_data_->vertices_.push_back(vert);
00815   }
00816 
00817   mesh_data_->mesh_center_ = sum / (double)(num_vertices);
00818   for (unsigned int j = 0 ; j < mesh_data_->vertices_.size() ; ++j)
00819   {
00820     double dist = (mesh_data_->vertices_[j] - mesh_data_->mesh_center_).squaredNorm();
00821     if (dist > mesh_data_->mesh_radiusB_)
00822       mesh_data_->mesh_radiusB_ = dist;
00823   }
00824 
00825   mesh_data_->mesh_radiusB_ = sqrt(mesh_data_->mesh_radiusB_);
00826   mesh_data_->triangles_.reserve(num_facets);
00827 
00828   //neccessary for qhull macro
00829   facetT * facet;
00830   FORALLfacets
00831   {
00832     Eigen::Vector4f planeEquation(facet->normal[0], facet->normal[1], facet->normal[2], facet->offset);
00833     if(!mesh_data_->planes_.empty()) {
00834         // filter equal planes - assuming same ones follow each other
00835         if((planeEquation - mesh_data_->planes_.back()).cwiseAbs().maxCoeff() > 1e-6)    // max diff to last
00836             mesh_data_->planes_.push_back(planeEquation);
00837     } else {
00838         mesh_data_->planes_.push_back(planeEquation);
00839     }
00840 
00841     // Needed by FOREACHvertex_i_
00842     int vertex_n, vertex_i;
00843     FOREACHvertex_i_ ((*facet).vertices)
00844     {
00845       mesh_data_->triangles_.push_back(qhull_vertex_table[vertex->id]);
00846     }
00847 
00848     mesh_data_->plane_for_triangle_[(mesh_data_->triangles_.size() - 1)/3] = mesh_data_->planes_.size() - 1;
00849   }
00850   qh_freeqhull(!qh_ALL);
00851   int curlong, totlong;
00852   qh_memfreeshort (&curlong, &totlong);
00853 }
00854 
00855 std::vector<double> bodies::ConvexMesh::getDimensions() const
00856 {
00857   return std::vector<double>();
00858 }
00859 
00860 void bodies::ConvexMesh::computeScaledVerticesFromPlaneProjections()
00861 {
00862     // compute the scaled vertices, if needed
00863     if (padding_ == 0.0 && scale_ == 1.0) {
00864         scaled_vertices_ = &mesh_data_->vertices_;
00865         return;
00866     }
00867 
00868     if (!scaled_vertices_storage_)
00869         scaled_vertices_storage_.reset(new EigenSTL::vector_Vector3d());
00870     scaled_vertices_ = scaled_vertices_storage_.get();
00871     scaled_vertices_storage_->resize(mesh_data_->vertices_.size());
00872     // project vertices along the vertex - center line to the scaled and padded plane
00873     // take the average of all tri's planes around that vertex as the result
00874     // is not unique
00875 
00876     // First figure out, which tris are connected to each vertex
00877     std::map<unsigned int, std::vector<unsigned int> > vertex_to_tris;
00878     for(unsigned int i = 0; i < mesh_data_->triangles_.size()/3; ++i) {
00879         vertex_to_tris[mesh_data_->triangles_[3*i + 0]].push_back(i);
00880         vertex_to_tris[mesh_data_->triangles_[3*i + 1]].push_back(i);
00881         vertex_to_tris[mesh_data_->triangles_[3*i + 2]].push_back(i);
00882     }
00883 
00884     for (unsigned int i = 0 ; i < mesh_data_->vertices_.size() ; ++i)
00885     {
00886         Eigen::Vector3d v(mesh_data_->vertices_[i] - mesh_data_->mesh_center_);
00887         EigenSTL::vector_Vector3d   projected_vertices;
00888         for(unsigned int t = 0; t < vertex_to_tris[i].size(); ++ t) {
00889             const Eigen::Vector4f & plane =
00890                 mesh_data_->planes_[mesh_data_->plane_for_triangle_[vertex_to_tris[i][t]]];
00891             Eigen::Vector3d plane_normal(plane.x(), plane.y(), plane.z());
00892             double d_scaled_padded = scale_ * plane.w()
00893                 - (1 - scale_) * mesh_data_->mesh_center_.dot(plane_normal) - padding_;
00894 
00895             // intersect vert - center with scaled/padded plane equation
00896             double denom = v.dot(plane_normal);
00897             if(fabs(denom) < 1e-3)
00898                 continue;
00899             double lambda = (-mesh_data_->mesh_center_.dot(plane_normal) - d_scaled_padded)/denom;
00900             Eigen::Vector3d vert_on_plane = v * lambda + mesh_data_->mesh_center_;
00901             projected_vertices.push_back(vert_on_plane);
00902         }
00903         if(projected_vertices.empty()) {
00904             double l = v.norm();
00905             scaled_vertices_storage_->at(i) = mesh_data_->mesh_center_ +
00906                 v * (scale_ + (l > detail::ZERO ? padding_ / l : 0.0));
00907         } else {
00908             Eigen::Vector3d sum(0,0,0);
00909             for(unsigned int v = 0; v < projected_vertices.size(); ++ v) {
00910                 sum += projected_vertices[v];
00911             }
00912             sum /= projected_vertices.size();
00913             scaled_vertices_storage_->at(i) = sum;
00914         }
00915     }
00916 }
00917 
00918 void bodies::ConvexMesh::updateInternalData()
00919 {
00920   if (!mesh_data_)
00921     return;
00922   Eigen::Affine3d pose = pose_;
00923   pose.translation() = Eigen::Vector3d(pose_ * mesh_data_->box_offset_);
00924 
00925   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()));
00926   bounding_box_.setDimensions(box_shape.get());
00927   bounding_box_.setPose(pose);
00928   bounding_box_.setPadding(padding_);
00929   bounding_box_.setScale(scale_);
00930 
00931   i_pose_ = pose_.inverse();
00932   center_ = pose_ * mesh_data_->mesh_center_;
00933   radiusB_ = mesh_data_->mesh_radiusB_ * scale_ + padding_;
00934   radiusBSqr_ = radiusB_ * radiusB_;
00935 
00936   // compute the scaled vertices, if needed
00937   if (padding_ == 0.0 && scale_ == 1.0)
00938     scaled_vertices_ = &mesh_data_->vertices_;
00939   else
00940   {
00941     if (!scaled_vertices_storage_)
00942       scaled_vertices_storage_.reset(new EigenSTL::vector_Vector3d());
00943     scaled_vertices_ = scaled_vertices_storage_.get();
00944     scaled_vertices_storage_->resize(mesh_data_->vertices_.size());
00945     for (unsigned int i = 0 ; i < mesh_data_->vertices_.size() ; ++i)
00946     {
00947       Eigen::Vector3d v(mesh_data_->vertices_[i] - mesh_data_->mesh_center_);
00948       double l = v.norm();
00949       scaled_vertices_storage_->at(i) = mesh_data_->mesh_center_ + v * (scale_ + (l > detail::ZERO ? padding_ / l : 0.0));
00950     }
00951   }
00952 }
00953 const std::vector<unsigned int>& bodies::ConvexMesh::getTriangles() const
00954 {
00955   static const std::vector<unsigned int> empty;
00956   return mesh_data_ ? mesh_data_->triangles_ : empty;
00957 }
00958 
00959 const EigenSTL::vector_Vector3d& bodies::ConvexMesh::getVertices() const
00960 {
00961   static const EigenSTL::vector_Vector3d empty;
00962   return mesh_data_ ? mesh_data_->vertices_ : empty;
00963 }
00964 
00965 const EigenSTL::vector_Vector3d& bodies::ConvexMesh::getScaledVertices() const
00966 {
00967   return scaled_vertices_ ? *scaled_vertices_ : getVertices();
00968 }
00969 
00970 boost::shared_ptr<bodies::Body> bodies::ConvexMesh::cloneAt(const Eigen::Affine3d &pose, double padding, double scale) const
00971 {
00972   ConvexMesh *m = new ConvexMesh();
00973   m->mesh_data_ = mesh_data_;
00974   m->padding_ = padding;
00975   m->scale_ = scale;
00976   m->pose_ = pose;
00977   m->updateInternalData();
00978   return boost::shared_ptr<Body>(m);
00979 }
00980 
00981 void bodies::ConvexMesh::computeBoundingSphere(BoundingSphere &sphere) const
00982 {
00983   sphere.center = center_;
00984   sphere.radius = radiusB_;
00985 }
00986 
00987 void bodies::ConvexMesh::computeBoundingCylinder(BoundingCylinder &cylinder) const
00988 {
00989   cylinder.length = mesh_data_ ? mesh_data_->bounding_cylinder_.length : 0.0;
00990   cylinder.radius = mesh_data_ ? mesh_data_->bounding_cylinder_.radius : 0.0;
00991   //need to do rotation correctly to get pose, which bounding box does
00992   BoundingCylinder cyl;
00993   bounding_box_.computeBoundingCylinder(cyl);
00994   cylinder.pose = cyl.pose;
00995 }
00996 
00997 bool bodies::ConvexMesh::isPointInsidePlanes(const Eigen::Vector3d& point) const
00998 {
00999   unsigned int numplanes = mesh_data_->planes_.size();
01000   for (unsigned int i = 0 ; i < numplanes ; ++i)
01001   {
01002     const Eigen::Vector4f& plane = mesh_data_->planes_[i];
01003     Eigen::Vector3d plane_vec(plane.x(), plane.y(), plane.z());
01004     double dist = plane_vec.dot(point) + plane.w() - padding_ - 1e-6;
01005     if (dist > 0.0)
01006       return false;
01007   }
01008   return true;
01009 }
01010 
01011 unsigned int bodies::ConvexMesh::countVerticesBehindPlane(const Eigen::Vector4f& planeNormal) const
01012 {
01013   unsigned int numvertices = mesh_data_->vertices_.size();
01014   unsigned int result = 0;
01015   for (unsigned int i = 0 ; i < numvertices ; ++i)
01016   {
01017     Eigen::Vector3d plane_vec(planeNormal.x(), planeNormal.y(), planeNormal.z());
01018     double dist = plane_vec.dot(mesh_data_->vertices_[i]) + planeNormal.w() - 1e-6;
01019     if (dist > 0.0)
01020       result++;
01021   }
01022   return result;
01023 }
01024 
01025 double bodies::ConvexMesh::computeVolume() const
01026 {
01027   double volume = 0.0;
01028   if (mesh_data_)
01029     for (unsigned int i = 0 ; i < mesh_data_->triangles_.size() / 3 ; ++i)
01030     {
01031       const Eigen::Vector3d &v1 = mesh_data_->vertices_[mesh_data_->triangles_[3*i + 0]];
01032       const Eigen::Vector3d &v2 = mesh_data_->vertices_[mesh_data_->triangles_[3*i + 1]];
01033       const Eigen::Vector3d &v3 = mesh_data_->vertices_[mesh_data_->triangles_[3*i + 2]];
01034       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();
01035     }
01036   return fabs(volume)/6.0;
01037 }
01038 
01039 bool bodies::ConvexMesh::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, EigenSTL::vector_Vector3d *intersections, unsigned int count) const
01040 {
01041   if (!mesh_data_) return false;
01042   if (detail::distanceSQR(center_, origin, dir) > radiusBSqr_) return false;
01043   if (!bounding_box_.intersectsRay(origin, dir)) return false;
01044 
01045   // transform the ray into the coordinate frame of the mesh
01046   Eigen::Vector3d orig(i_pose_ * origin);
01047   Eigen::Vector3d dr(i_pose_ * dir);
01048 
01049   std::vector<detail::intersc> ipts;
01050 
01051   bool result = false;
01052 
01053   // for each triangle
01054   const unsigned int nt = mesh_data_->triangles_.size() / 3;
01055   for (unsigned int i = 0 ; i < nt ; ++i)
01056   {
01057     Eigen::Vector3d vec(mesh_data_->planes_[mesh_data_->plane_for_triangle_[i]].x(),
01058                         mesh_data_->planes_[mesh_data_->plane_for_triangle_[i]].y(),
01059                         mesh_data_->planes_[mesh_data_->plane_for_triangle_[i]].z());
01060 
01061     double tmp = vec.dot(dr);
01062     if (fabs(tmp) > detail::ZERO)
01063     {
01064 
01065       double t = -(vec.dot(orig) + mesh_data_->planes_[mesh_data_->plane_for_triangle_[i]].w()) / tmp;
01066       if (t > 0.0)
01067       {
01068         const int i3 = 3 * i;
01069         const int v1 = mesh_data_->triangles_[i3 + 0];
01070         const int v2 = mesh_data_->triangles_[i3 + 1];
01071         const int v3 = mesh_data_->triangles_[i3 + 2];
01072 
01073         const Eigen::Vector3d &a = scaled_vertices_->at(v1);
01074         const Eigen::Vector3d &b = scaled_vertices_->at(v2);
01075         const Eigen::Vector3d &c = scaled_vertices_->at(v3);
01076 
01077         Eigen::Vector3d cb(c - b);
01078         Eigen::Vector3d ab(a - b);
01079 
01080         // intersection of the plane defined by the triangle and the ray
01081         Eigen::Vector3d P(orig + dr * t);
01082 
01083         // check if it is inside the triangle
01084         Eigen::Vector3d pb(P - b);
01085         Eigen::Vector3d c1(cb.cross(pb));
01086         Eigen::Vector3d c2(cb.cross(ab));
01087         if (c1.dot(c2) < 0.0)
01088           continue;
01089 
01090         Eigen::Vector3d ca(c - a);
01091         Eigen::Vector3d pa(P - a);
01092         Eigen::Vector3d ba(-ab);
01093 
01094         c1 = ca.cross(pa);
01095         c2 = ca.cross(ba);
01096         if (c1.dot(c2) < 0.0)
01097           continue;
01098 
01099         c1 = ba.cross(pa);
01100         c2 = ba.cross(ca);
01101 
01102         if (c1.dot(c2) < 0.0)
01103           continue;
01104 
01105         result = true;
01106         if (intersections)
01107         {
01108           detail::intersc ip(origin + dir * t, t);
01109           ipts.push_back(ip);
01110         }
01111         else
01112           break;
01113       }
01114     }
01115   }
01116 
01117   if (intersections)
01118   {
01119     std::sort(ipts.begin(), ipts.end(), detail::interscOrder());
01120     const unsigned int n = count > 0 ? std::min<unsigned int>(count, ipts.size()) : ipts.size();
01121     for (unsigned int i = 0 ; i < n ; ++i)
01122       intersections->push_back(ipts[i].pt);
01123   }
01124 
01125   return result;
01126 }
01127 
01128 bodies::BodyVector::BodyVector()
01129 {
01130 }
01131 
01132 bodies::BodyVector::BodyVector(const std::vector<shapes::Shape*>& shapes,
01133                                const EigenSTL::vector_Affine3d& poses,
01134                                double padding)
01135 {
01136   for(unsigned int i = 0; i < shapes.size(); i++)
01137     addBody(shapes[i], poses[i], padding);
01138 }
01139 
01140 
01141 bodies::BodyVector::~BodyVector()
01142 {
01143   clear();
01144 }
01145 
01146 void bodies::BodyVector::clear()
01147 {
01148   for(unsigned int i = 0; i < bodies_.size(); i++)
01149     delete bodies_[i];
01150   bodies_.clear();
01151 }
01152 
01153 void bodies::BodyVector::addBody(Body *body)
01154 {
01155   bodies_.push_back(body);
01156   BoundingSphere sphere;
01157   body->computeBoundingSphere(sphere);
01158 }
01159 
01160 void bodies::BodyVector::addBody(const shapes::Shape *shape, const Eigen::Affine3d& pose, double padding)
01161 {
01162   bodies::Body* body = bodies::createBodyFromShape(shape);
01163   body->setPose(pose);
01164   body->setPadding(padding);
01165   addBody(body);
01166 }
01167 
01168 std::size_t bodies::BodyVector::getCount() const
01169 {
01170   return bodies_.size();
01171 }
01172 
01173 void bodies::BodyVector::setPose(unsigned int i, const Eigen::Affine3d& pose)
01174 {
01175   if (i >= bodies_.size())
01176   {
01177     logError("There is no body at index %u", i);
01178     return;
01179   }
01180 
01181   bodies_[i]->setPose(pose);
01182 }
01183 
01184 const bodies::Body* bodies::BodyVector::getBody(unsigned int i) const
01185 {
01186   if (i >= bodies_.size())
01187   {
01188     logError("There is no body at index %u", i);
01189     return NULL;
01190   }
01191   else
01192     return bodies_[i];
01193 }
01194 
01195 bool bodies::BodyVector::containsPoint(const Eigen::Vector3d &p, std::size_t &index, bool verbose) const
01196 {
01197   for (std::size_t i = 0 ; i < bodies_.size() ; ++i)
01198     if (bodies_[i]->containsPoint(p, verbose))
01199     {
01200       index = i;
01201       return true;
01202     }
01203   return false;
01204 }
01205 
01206 bool bodies::BodyVector::containsPoint(const Eigen::Vector3d &p, bool verbose) const
01207 {
01208   std::size_t dummy;
01209   return containsPoint(p, dummy, verbose);
01210 }
01211 
01212 bool bodies::BodyVector::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d &dir, std::size_t &index, EigenSTL::vector_Vector3d *intersections, unsigned int count) const
01213 {
01214   for (std::size_t i = 0 ; i < bodies_.size() ; ++i)
01215     if (bodies_[i]->intersectsRay(origin, dir, intersections, count))
01216     {
01217       index = i;
01218       return true;
01219     }
01220   return false;
01221 }


geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Thu Jun 6 2019 20:13:52