00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
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
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)
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
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)
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
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
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
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
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 , 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)
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
00678 Eigen::Vector3d tri_normal = d1.cross(d2);
00679 tri_normal.normalize();
00680
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
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
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
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
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
00835 if((planeEquation - mesh_data_->planes_.back()).cwiseAbs().maxCoeff() > 1e-6)
00836 mesh_data_->planes_.push_back(planeEquation);
00837 } else {
00838 mesh_data_->planes_.push_back(planeEquation);
00839 }
00840
00841
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
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
00873
00874
00875
00876
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
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
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
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
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
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
01081 Eigen::Vector3d P(orig + dr * t);
01082
01083
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 }