43 #ifdef GEOMETRIC_SHAPES_HAVE_QHULL_2011 44 #include <libqhull/libqhull.h> 45 #include <libqhull/mem.h> 46 #include <libqhull/qset.h> 47 #include <libqhull/geom.h> 48 #include <libqhull/merge.h> 49 #include <libqhull/poly.h> 50 #include <libqhull/io.h> 51 #include <libqhull/stat.h> 53 #include <qhull/qhull.h> 54 #include <qhull/mem.h> 55 #include <qhull/qset.h> 56 #include <qhull/geom.h> 57 #include <qhull/merge.h> 58 #include <qhull/poly.h> 60 #include <qhull/stat.h> 64 #include <boost/math/constants/constants.hpp> 68 #include <Eigen/Geometry> 74 static const double ZERO = 1e-9;
78 static inline double distanceSQR(
const Eigen::Vector3d& p,
const Eigen::Vector3d& origin,
const Eigen::Vector3d& dir)
80 Eigen::Vector3d a = p - origin;
81 double d = dir.normalized().dot(a);
82 return a.squaredNorm() - d * d;
88 intersc(
const Eigen::Vector3d& _pt,
const double _tm) :
pt(_pt),
time(_tm)
95 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
111 useDimensions(shape);
112 updateInternalData();
116 Eigen::Vector3d& result)
120 for (
unsigned int i = 0; i < max_attempts; ++i)
125 if (containsPoint(result))
133 return (center_ - p).squaredNorm() < radius2_;
143 std::vector<double>
d(1, radius_);
149 radiusU_ = radius_ * scale_ + padding_;
150 radius2_ = radiusU_ * radiusU_;
151 center_ = pose_.translation();
162 return std::shared_ptr<Body>(s);
167 return 4.0 * boost::math::constants::pi<double>() * radiusU_ * radiusU_ * radiusU_ / 3.0;
178 cylinder.
pose = pose_;
179 cylinder.
radius = radiusU_;
180 cylinder.
length = radiusU_;
184 Eigen::Vector3d& result)
186 for (
unsigned int i = 0; i < max_attempts; ++i)
188 const double minX = center_.x() - radiusU_;
189 const double maxX = center_.x() + radiusU_;
190 const double minY = center_.y() - radiusU_;
191 const double maxY = center_.y() + radiusU_;
192 const double minZ = center_.z() - radiusU_;
193 const double maxZ = center_.z() + radiusU_;
196 for (
int j = 0; j < 20; ++j)
199 if (containsPoint(result))
214 Eigen::Vector3d cp = origin - center_;
215 double dpcpv = cp.dot(dir);
217 Eigen::Vector3d w = cp - dpcpv * dir;
218 Eigen::Vector3d Q = center_ + w;
219 double x = radius2_ - w.squaredNorm();
224 double dpQv = w.dot(dir);
228 intersections->push_back(Q);
236 Eigen::Vector3d A = Q - w;
237 Eigen::Vector3d B = Q + w;
239 double dpAv = w.dot(dir);
241 double dpBv = w.dot(dir);
248 intersections->push_back(A);
258 intersections->push_back(B);
266 Eigen::Vector3d v = p - center_;
267 double pH = v.dot(normalH_);
269 if (fabs(pH) > length2_)
272 double pB1 = v.dot(normalB1_);
273 double remaining = radius2_ - pB1 * pB1;
279 double pB2 = v.dot(normalB2_);
280 return pB2 * pB2 < remaining;
292 std::vector<double>
d(2);
300 radiusU_ = radius_ * scale_ + padding_;
301 radius2_ = radiusU_ * radiusU_;
302 length2_ = scale_ * length_ / 2.0 + padding_;
303 center_ = pose_.translation();
304 radiusBSqr_ = length2_ * length2_ + radius2_;
305 radiusB_ = sqrt(radiusBSqr_);
307 Eigen::Matrix3d basis = pose_.rotation();
308 normalB1_ = basis.col(0);
309 normalB2_ = basis.col(1);
310 normalH_ = basis.col(2);
312 double tmp = -normalH_.dot(center_);
313 d1_ = tmp + length2_;
314 d2_ = tmp - length2_;
318 Eigen::Vector3d& result)
321 double a = rng.
uniformReal(-boost::math::constants::pi<double>(), boost::math::constants::pi<double>());
323 double x = cos(a) * r;
324 double y = sin(a) * r;
329 result = Eigen::Vector3d(x, y, z);
342 return std::shared_ptr<Body>(c);
347 return 2.0 * boost::math::constants::pi<double>() * radius2_ * length2_;
358 cylinder.
pose = pose_;
359 cylinder.
radius = radiusU_;
360 cylinder.
length = scale_ * length_ + padding_;
369 std::vector<detail::intersc> ipts;
372 double tmp = normalH_.dot(dir);
375 double tmp2 = -normalH_.dot(origin);
376 double t1 = (tmp2 - d1_) / tmp;
380 Eigen::Vector3d p1(origin + dir * t1);
381 Eigen::Vector3d v1(p1 - center_);
382 v1 = v1 - normalH_.dot(v1) * normalH_;
385 if (intersections == NULL)
393 double t2 = (tmp2 - d2_) / tmp;
396 Eigen::Vector3d p2(origin + dir * t2);
397 Eigen::Vector3d v2(p2 - center_);
398 v2 = v2 - normalH_.dot(v2) * normalH_;
401 if (intersections == NULL)
413 Eigen::Vector3d VD(normalH_.cross(dir));
414 Eigen::Vector3d ROD(normalH_.cross(origin - center_));
415 double a = VD.squaredNorm();
416 double b = 2.0 * ROD.dot(VD);
417 double c = ROD.squaredNorm() - radius2_;
418 double d = b * b - 4.0 * a * c;
423 double t1 = (b + d) / e;
424 double t2 = (b - d) / e;
428 Eigen::Vector3d p1(origin + dir * t1);
429 Eigen::Vector3d v1(center_ - p1);
433 if (intersections == NULL)
443 Eigen::Vector3d p2(origin + dir * t2);
444 Eigen::Vector3d v2(center_ - p2);
448 if (intersections == NULL)
461 const unsigned int n = count > 0 ? std::min<unsigned int>(count, ipts.size()) : ipts.size();
462 for (
unsigned int i = 0; i < n; ++i)
463 intersections->push_back(ipts[i].pt);
469 Eigen::Vector3d& result)
471 result = pose_ * Eigen::Vector3d(rng.
uniformReal(-length2_, length2_), rng.
uniformReal(-width2_, width2_),
478 Eigen::Vector3d v = p - center_;
479 double pL = v.dot(normalL_);
480 if (fabs(pL) > length2_)
483 double pW = v.dot(normalW_);
484 if (fabs(pW) > width2_)
487 double pH = v.dot(normalH_);
488 if (fabs(pH) > height2_)
496 const double* size =
static_cast<const shapes::Box*
>(shape)->size;
504 std::vector<double>
d(3);
513 double s2 = scale_ / 2.0;
514 length2_ = length_ * s2 + padding_;
515 width2_ = width_ * s2 + padding_;
516 height2_ = height_ * s2 + padding_;
518 center_ = pose_.translation();
520 radius2_ = length2_ * length2_ + width2_ * width2_ + height2_ * height2_;
521 radiusB_ = sqrt(radius2_);
523 Eigen::Matrix3d basis = pose_.rotation();
524 normalL_ = basis.col(0);
525 normalW_ = basis.col(1);
526 normalH_ = basis.col(2);
528 const Eigen::Vector3d tmp(normalL_ * length2_ + normalW_ * width2_ + normalH_ * height2_);
529 corner1_ = center_ - tmp;
530 corner2_ = center_ + tmp;
533 std::shared_ptr<bodies::Body>
bodies::Box::cloneAt(
const Eigen::Affine3d& pose,
double padding,
double scale)
const 543 return std::shared_ptr<Body>(b);
548 return 8.0 * length2_ * width2_ * height2_;
561 if (length2_ > width2_ && length2_ > height2_)
563 cylinder.
length = length2_ * 2.0;
566 Eigen::Affine3d rot(Eigen::AngleAxisd(90.0
f * (
M_PI / 180.0
f), Eigen::Vector3d::UnitY()));
567 cylinder.
pose = pose_ * rot;
569 else if (width2_ > height2_)
571 cylinder.
length = width2_ * 2.0;
574 cylinder.
radius = sqrt(height2_ * height2_ + length2_ * length2_);
575 Eigen::Affine3d rot(Eigen::AngleAxisd(90.0
f * (
M_PI / 180.0
f), Eigen::Vector3d::UnitX()));
576 cylinder.
pose = pose_ * rot;
580 cylinder.
length = height2_ * 2.0;
583 cylinder.
pose = pose_;
585 cylinder.
radius = sqrt(a * a + b * b);
592 float tmin, tmax, tymin, tymax, tzmin, tzmax;
593 float divx, divy, divz;
597 tmin = (corner1_.x() - origin.x()) * divx;
598 tmax = (corner2_.x() - origin.x()) * divx;
602 tmax = (corner1_.x() - origin.x()) * divx;
603 tmin = (corner2_.x() - origin.x()) * divx;
609 tymin = (corner1_.y() - origin.y()) * divy;
610 tymax = (corner2_.y() - origin.y()) * divy;
614 tymax = (corner1_.y() - origin.y()) * divy;
615 tymin = (corner2_.y() - origin.y()) * divy;
618 if ((tmin > tymax || tymin > tmax))
629 tzmin = (corner1_.z() - origin.z()) * divz;
630 tzmax = (corner2_.z() - origin.z()) * divz;
634 tzmax = (corner1_.z() - origin.z()) * divz;
635 tzmin = (corner2_.z() - origin.z()) * divz;
638 if ((tmin > tzmax || tzmin > tmax))
653 intersections->push_back(tmin * dir + origin);
655 intersections->push_back(tmax * dir + origin);
658 intersections->push_back(tmax * dir + origin);
668 if (bounding_box_.containsPoint(p))
670 Eigen::Vector3d ip(i_pose_ * p);
671 ip = mesh_data_->mesh_center_ + (ip - mesh_data_->mesh_center_) / scale_;
672 return isPointInsidePlanes(ip);
680 for (
unsigned int i = 0; i < mesh_data_->triangles_.size(); i += 3)
683 mesh_data_->vertices_[mesh_data_->triangles_[i]] - mesh_data_->vertices_[mesh_data_->triangles_[i + 1]];
685 mesh_data_->vertices_[mesh_data_->triangles_[i]] - mesh_data_->vertices_[mesh_data_->triangles_[i + 2]];
687 Eigen::Vector3d tri_normal = d1.cross(d2);
688 tri_normal.normalize();
690 Eigen::Vector3d normal(mesh_data_->planes_[mesh_data_->plane_for_triangle_[i / 3]].x(),
691 mesh_data_->planes_[mesh_data_->plane_for_triangle_[i / 3]].y(),
692 mesh_data_->planes_[mesh_data_->plane_for_triangle_[i / 3]].z());
693 bool same_dir = tri_normal.dot(normal) > 0;
696 std::swap(mesh_data_->triangles_[i], mesh_data_->triangles_[i + 1]);
706 double maxX = -std::numeric_limits<double>::infinity(), maxY = -std::numeric_limits<double>::infinity(),
707 maxZ = -std::numeric_limits<double>::infinity();
708 double minX = std::numeric_limits<double>::infinity(), minY = std::numeric_limits<double>::infinity(),
709 minZ = std::numeric_limits<double>::infinity();
714 double vy = mesh->
vertices[3 * i + 1];
715 double vz = mesh->
vertices[3 * i + 2];
739 mesh_data_->box_size_ = Eigen::Vector3d(maxX - minX, maxY - minY, maxZ - minZ);
741 mesh_data_->box_offset_ = Eigen::Vector3d((minX + maxX) / 2.0, (minY + maxY) / 2.0, (minZ + maxZ) / 2.0);
743 mesh_data_->planes_.clear();
744 mesh_data_->triangles_.clear();
745 mesh_data_->vertices_.clear();
746 mesh_data_->mesh_radiusB_ = 0.0;
747 mesh_data_->mesh_center_ = Eigen::Vector3d();
749 double xdim = maxX - minX;
750 double ydim = maxY - minY;
751 double zdim = maxZ - minZ;
761 double maxdist = -std::numeric_limits<double>::infinity();
762 if (xdim > ydim && xdim > zdim)
766 pose1 = mesh_data_->box_offset_.y();
767 pose2 = mesh_data_->box_offset_.z();
770 else if (ydim > zdim)
774 pose1 = mesh_data_->box_offset_.x();
775 pose2 = mesh_data_->box_offset_.z();
782 pose1 = mesh_data_->box_offset_.x();
783 pose2 = mesh_data_->box_offset_.y();
788 coordT* points = (coordT*)calloc(mesh->
vertex_count * 3,
sizeof(coordT));
791 points[3 * i + 0] = (coordT)mesh->
vertices[3 * i + 0];
792 points[3 * i + 1] = (coordT)mesh->
vertices[3 * i + 1];
793 points[3 * i + 2] = (coordT)mesh->
vertices[3 * i + 2];
795 double dista = mesh->
vertices[3 * i + off1] - pose1;
796 double distb = mesh->
vertices[3 * i + off2] - pose2;
797 double dist = sqrt(((dista * dista) + (distb * distb)));
801 mesh_data_->bounding_cylinder_.radius = maxdist;
802 mesh_data_->bounding_cylinder_.length = cyl_length;
804 static FILE* null = fopen(
"/dev/null",
"w");
806 char flags[] =
"qhull Tv Qt";
807 int exitcode = qh_new_qhull(3, mesh->
vertex_count, points,
true, flags, null, null);
812 qh_freeqhull(!qh_ALL);
813 int curlong, totlong;
814 qh_memfreeshort(&curlong, &totlong);
818 int num_facets = qh num_facets;
820 int num_vertices = qh num_vertices;
821 mesh_data_->vertices_.reserve(num_vertices);
822 Eigen::Vector3d sum(0, 0, 0);
825 std::map<unsigned int, unsigned int> qhull_vertex_table;
829 Eigen::Vector3d vert(vertex->point[0], vertex->point[1], vertex->point[2]);
830 qhull_vertex_table[vertex->id] = mesh_data_->vertices_.size();
832 mesh_data_->vertices_.push_back(vert);
835 mesh_data_->mesh_center_ = sum / (double)(num_vertices);
836 for (
unsigned int j = 0; j < mesh_data_->vertices_.size(); ++j)
838 double dist = (mesh_data_->vertices_[j] - mesh_data_->mesh_center_).squaredNorm();
839 if (dist > mesh_data_->mesh_radiusB_)
840 mesh_data_->mesh_radiusB_ = dist;
843 mesh_data_->mesh_radiusB_ = sqrt(mesh_data_->mesh_radiusB_);
844 mesh_data_->triangles_.reserve(num_facets);
850 Eigen::Vector4f planeEquation(facet->normal[0], facet->normal[1], facet->normal[2], facet->offset);
851 if (!mesh_data_->planes_.empty())
854 if ((planeEquation - mesh_data_->planes_.back()).cwiseAbs().maxCoeff() > 1e-6)
855 mesh_data_->planes_.push_back(planeEquation);
859 mesh_data_->planes_.push_back(planeEquation);
863 int vertex_n, vertex_i;
864 FOREACHvertex_i_((*facet).vertices)
866 mesh_data_->triangles_.push_back(qhull_vertex_table[vertex->id]);
869 mesh_data_->plane_for_triangle_[(mesh_data_->triangles_.size() - 1) / 3] = mesh_data_->planes_.size() - 1;
871 qh_freeqhull(!qh_ALL);
872 int curlong, totlong;
873 qh_memfreeshort(&curlong, &totlong);
878 return std::vector<double>();
884 if (padding_ == 0.0 && scale_ == 1.0)
886 scaled_vertices_ = &mesh_data_->vertices_;
890 if (!scaled_vertices_storage_)
892 scaled_vertices_ = scaled_vertices_storage_.get();
893 scaled_vertices_storage_->resize(mesh_data_->vertices_.size());
899 std::map<unsigned int, std::vector<unsigned int> > vertex_to_tris;
900 for (
unsigned int i = 0; i < mesh_data_->triangles_.size() / 3; ++i)
902 vertex_to_tris[mesh_data_->triangles_[3 * i + 0]].push_back(i);
903 vertex_to_tris[mesh_data_->triangles_[3 * i + 1]].push_back(i);
904 vertex_to_tris[mesh_data_->triangles_[3 * i + 2]].push_back(i);
907 for (
unsigned int i = 0; i < mesh_data_->vertices_.size(); ++i)
909 Eigen::Vector3d v(mesh_data_->vertices_[i] - mesh_data_->mesh_center_);
911 for (
unsigned int t = 0; t < vertex_to_tris[i].size(); ++t)
913 const Eigen::Vector4f& plane = mesh_data_->planes_[mesh_data_->plane_for_triangle_[vertex_to_tris[i][t]]];
914 Eigen::Vector3d plane_normal(plane.x(), plane.y(), plane.z());
915 double d_scaled_padded =
916 scale_ * plane.w() - (1 - scale_) * mesh_data_->mesh_center_.dot(plane_normal) - padding_;
919 double denom = v.dot(plane_normal);
920 if (fabs(denom) < 1e-3)
922 double lambda = (-mesh_data_->mesh_center_.dot(plane_normal) - d_scaled_padded) / denom;
923 Eigen::Vector3d vert_on_plane = v * lambda + mesh_data_->mesh_center_;
924 projected_vertices.push_back(vert_on_plane);
926 if (projected_vertices.empty())
929 scaled_vertices_storage_->at(i) =
930 mesh_data_->mesh_center_ + v * (scale_ + (l >
detail::ZERO ? padding_ / l : 0.0));
934 Eigen::Vector3d sum(0, 0, 0);
935 for (
unsigned int v = 0; v < projected_vertices.size(); ++v)
937 sum += projected_vertices[v];
939 sum /= projected_vertices.size();
940 scaled_vertices_storage_->at(i) = sum;
949 Eigen::Affine3d pose = pose_;
950 pose.translation() = Eigen::Vector3d(pose_ * mesh_data_->box_offset_);
952 std::unique_ptr<shapes::Box> box_shape(
953 new shapes::Box(mesh_data_->box_size_.x(), mesh_data_->box_size_.y(), mesh_data_->box_size_.z()));
954 bounding_box_.setDimensions(box_shape.get());
955 bounding_box_.setPose(pose);
956 bounding_box_.setPadding(padding_);
957 bounding_box_.setScale(scale_);
959 i_pose_ = pose_.inverse();
960 center_ = pose_ * mesh_data_->mesh_center_;
961 radiusB_ = mesh_data_->mesh_radiusB_ * scale_ + padding_;
962 radiusBSqr_ = radiusB_ * radiusB_;
965 if (padding_ == 0.0 && scale_ == 1.0)
966 scaled_vertices_ = &mesh_data_->vertices_;
969 if (!scaled_vertices_storage_)
971 scaled_vertices_ = scaled_vertices_storage_.get();
972 scaled_vertices_storage_->resize(mesh_data_->vertices_.size());
973 for (
unsigned int i = 0; i < mesh_data_->vertices_.size(); ++i)
975 Eigen::Vector3d v(mesh_data_->vertices_[i] - mesh_data_->mesh_center_);
977 scaled_vertices_storage_->at(i) =
978 mesh_data_->mesh_center_ + v * (scale_ + (l >
detail::ZERO ? padding_ / l : 0.0));
984 static const std::vector<unsigned int> empty;
985 return mesh_data_ ? mesh_data_->triangles_ : empty;
991 return mesh_data_ ? mesh_data_->vertices_ : empty;
996 return scaled_vertices_ ? *scaled_vertices_ : getVertices();
1008 return std::shared_ptr<Body>(m);
1014 sphere.
radius = radiusB_;
1019 cylinder.
length = mesh_data_ ? mesh_data_->bounding_cylinder_.length : 0.0;
1020 cylinder.
radius = mesh_data_ ? mesh_data_->bounding_cylinder_.radius : 0.0;
1023 bounding_box_.computeBoundingCylinder(cyl);
1029 unsigned int numplanes = mesh_data_->planes_.size();
1030 for (
unsigned int i = 0; i < numplanes; ++i)
1032 const Eigen::Vector4f& plane = mesh_data_->planes_[i];
1033 Eigen::Vector3d plane_vec(plane.x(), plane.y(), plane.z());
1034 double dist = plane_vec.dot(point) + plane.w() - padding_ - 1e-6;
1043 unsigned int numvertices = mesh_data_->vertices_.size();
1044 unsigned int result = 0;
1045 for (
unsigned int i = 0; i < numvertices; ++i)
1047 Eigen::Vector3d plane_vec(planeNormal.x(), planeNormal.y(), planeNormal.z());
1048 double dist = plane_vec.dot(mesh_data_->vertices_[i]) + planeNormal.w() - 1e-6;
1057 double volume = 0.0;
1059 for (
unsigned int i = 0; i < mesh_data_->triangles_.size() / 3; ++i)
1061 const Eigen::Vector3d& v1 = mesh_data_->vertices_[mesh_data_->triangles_[3 * i + 0]];
1062 const Eigen::Vector3d& v2 = mesh_data_->vertices_[mesh_data_->triangles_[3 * i + 1]];
1063 const Eigen::Vector3d& v3 = mesh_data_->vertices_[mesh_data_->triangles_[3 * i + 2]];
1064 volume += v1.x() * v2.y() * v3.z() + v2.x() * v3.y() * v1.z() + v3.x() * v1.y() * v2.z() -
1065 v1.x() * v3.y() * v2.z() - v2.x() * v1.y() * v3.z() - v3.x() * v2.y() * v1.z();
1067 return fabs(volume) / 6.0;
1077 if (!bounding_box_.intersectsRay(origin, dir))
1081 Eigen::Vector3d orig(i_pose_ * origin);
1082 Eigen::Vector3d dr(i_pose_ * dir);
1084 std::vector<detail::intersc> ipts;
1086 bool result =
false;
1089 const unsigned int nt = mesh_data_->triangles_.size() / 3;
1090 for (
unsigned int i = 0; i < nt; ++i)
1092 Eigen::Vector3d vec(mesh_data_->planes_[mesh_data_->plane_for_triangle_[i]].x(),
1093 mesh_data_->planes_[mesh_data_->plane_for_triangle_[i]].y(),
1094 mesh_data_->planes_[mesh_data_->plane_for_triangle_[i]].z());
1096 double tmp = vec.dot(dr);
1099 double t = -(vec.dot(orig) + mesh_data_->planes_[mesh_data_->plane_for_triangle_[i]].w()) / tmp;
1102 const int i3 = 3 * i;
1103 const int v1 = mesh_data_->triangles_[i3 + 0];
1104 const int v2 = mesh_data_->triangles_[i3 + 1];
1105 const int v3 = mesh_data_->triangles_[i3 + 2];
1107 const Eigen::Vector3d& a = scaled_vertices_->at(v1);
1108 const Eigen::Vector3d& b = scaled_vertices_->at(v2);
1109 const Eigen::Vector3d& c = scaled_vertices_->at(v3);
1111 Eigen::Vector3d cb(c - b);
1112 Eigen::Vector3d ab(a - b);
1115 Eigen::Vector3d P(orig + dr * t);
1118 Eigen::Vector3d pb(P - b);
1119 Eigen::Vector3d c1(cb.cross(pb));
1120 Eigen::Vector3d c2(cb.cross(ab));
1121 if (c1.dot(c2) < 0.0)
1124 Eigen::Vector3d ca(c - a);
1125 Eigen::Vector3d pa(P - a);
1126 Eigen::Vector3d ba(-ab);
1130 if (c1.dot(c2) < 0.0)
1136 if (c1.dot(c2) < 0.0)
1154 const unsigned int n = count > 0 ? std::min<unsigned int>(count, ipts.size()) : ipts.size();
1155 for (
unsigned int i = 0; i < n; ++i)
1156 intersections->push_back(ipts[i].pt);
1169 for (
unsigned int i = 0; i < shapes.size(); i++)
1170 addBody(shapes[i], poses[i], padding);
1180 for (
unsigned int i = 0; i < bodies_.size(); i++)
1187 bodies_.push_back(body);
1202 return bodies_.size();
1207 if (i >= bodies_.size())
1213 bodies_[i]->setPose(pose);
1218 if (i >= bodies_.size())
1229 for (std::size_t i = 0; i < bodies_.size(); ++i)
1230 if (bodies_[i]->containsPoint(p, verbose))
1241 return containsPoint(p, dummy, verbose);
1247 for (std::size_t i = 0; i < bodies_.size(); ++i)
1248 if (bodies_[i]->intersectsRay(origin, dir, intersections, count))
#define CONSOLE_BRIDGE_logWarn(fmt,...)
Definition of various shapes. No properties such as position are included. These are simply the descr...
void setPose(const Eigen::Affine3d &pose)
Set the pose of the body. Default is identity.
virtual void updateInternalData()
This function is called every time a change to the body is made, so that intermediate values stored f...
const std::vector< unsigned int > & getTriangles() const
virtual bool intersectsRay(const Eigen::Vector3d &origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections=NULL, unsigned int count=0) const
Check if a ray intersects the body, and find the set of intersections, in order, along the ray...
virtual void useDimensions(const shapes::Shape *shape)
Depending on the shape, this function copies the relevant data to the body.
virtual bool intersectsRay(const Eigen::Vector3d &origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections=NULL, unsigned int count=0) const
Check if a ray intersects the body, and find the set of intersections, in order, along the ray...
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
Definition of a cylinder Length is along z axis. Origin is at center of mass.
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
virtual std::vector< double > getDimensions() const
Returns an empty vector.
virtual bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const
Check if a point is inside the body.
Definition of a cylinder.
virtual bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result)
Sample a point that is included in the body using a given random number generator.
virtual void computeBoundingSphere(BoundingSphere &sphere) const
Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for...
virtual void useDimensions(const shapes::Shape *shape)
Depending on the shape, this function copies the relevant data to the body.
virtual std::vector< double > getDimensions() const
Get the length & width & height (x, y, z) of the box.
virtual void computeBoundingSphere(BoundingSphere &sphere) const
Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for...
virtual bool intersectsRay(const Eigen::Vector3d &origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections=NULL, unsigned int count=0) const
Check if a ray intersects the body, and find the set of intersections, in order, along the ray...
virtual bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result)
Sample a point that is included in the body using a given random number generator.
static double distanceSQR(const Eigen::Vector3d &p, const Eigen::Vector3d &origin, const Eigen::Vector3d &dir)
Compute the square of the distance between a ray and a point Note: this requires 'dir' to be normaliz...
intersc(const Eigen::Vector3d &_pt, const double _tm)
void setPose(unsigned int i, const Eigen::Affine3d &pose)
Set the pose of a particular body in the vector of bodies.
std::shared_ptr< MeshData > mesh_data_
virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const
Compute the bounding cylinder for the body, in its current pose. Scaling and padding are accounted fo...
virtual bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const
Check if a point is inside the body.
bool intersectsRay(const Eigen::Vector3d &origin, const Eigen::Vector3d &dir, std::size_t &index, EigenSTL::vector_Vector3d *intersections=NULL, unsigned int count=0) const
Check if any of the bodies intersects the ray defined by origin and dir. When the first intersection ...
virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const
Compute the bounding cylinder for the body, in its current pose. Scaling and padding are accounted fo...
bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const
Check if any body in the vector contains the input point.
virtual bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const
Check if a point is inside the body.
virtual void computeBoundingSphere(BoundingSphere &sphere) const
Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for...
virtual void useDimensions(const shapes::Shape *shape)
Depending on the shape, this function copies the relevant data to the body.
bool operator()(const intersc &a, const intersc &b) const
Definition of a sphere that bounds another object.
virtual BodyPtr cloneAt(const Eigen::Affine3d &pose, double padding, double scale) const
Get a clone of this body, but one that is located at the pose pose and has possibly different passing...
virtual double computeVolume() const
Compute the volume of the body. This method includes changes induced by scaling and padding...
virtual BodyPtr cloneAt(const Eigen::Affine3d &pose, double padding, double scale) const
Get a clone of this body, but one that is located at the pose pose and has possibly different passing...
unsigned int vertex_count
The number of available vertices.
A basic definition of a shape. Shapes are considered centered at origin.
virtual void updateInternalData()
This function is called every time a change to the body is made, so that intermediate values stored f...
virtual double computeVolume() const
Compute the volume of the body. This method includes changes induced by scaling and padding...
Definition of a convex mesh. Convex hull is computed for a given shape::Mesh.
unsigned int countVerticesBehindPlane(const Eigen::Vector4f &planeNormal) const
(Used mainly for debugging) Count the number of vertices behind a plane
virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const
Compute the bounding cylinder for the body, in its current pose. Scaling and padding are accounted fo...
const EigenSTL::vector_Vector3d & getVertices() const
virtual std::vector< double > getDimensions() const
Get the radius & length of the cylinder.
Definition of a cylinder.
Body * createBodyFromShape(const shapes::Shape *shape)
Create a body from a given shape.
Eigen::Affine3d pose_
The location of the body (position and orientation)
virtual BodyPtr cloneAt(const Eigen::Affine3d &pose, double padding, double scale) const
Get a clone of this body, but one that is located at the pose pose and has possibly different passing...
virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const
Compute the bounding cylinder for the body, in its current pose. Scaling and padding are accounted fo...
double * vertices
The position for each vertex vertex k has values at index (3k, 3k+1, 3k+2) = (x,y,z)
double padding_
The scale that was set for this body.
void clear()
Clear all bodies from the vector.
virtual bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result)
Sample a point that is included in the body using a given random number generator.
double uniformReal(double lower_bound, double upper_bound)
virtual bool intersectsRay(const Eigen::Vector3d &origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections=NULL, unsigned int count=0) const
Check if a ray intersects the body, and find the set of intersections, in order, along the ray...
virtual void updateInternalData()
This function is called every time a change to the body is made, so that intermediate values stored f...
virtual void updateInternalData()
This function is called every time a change to the body is made, so that intermediate values stored f...
virtual void useDimensions(const shapes::Shape *shape)
Depending on the shape, this function copies the relevant data to the body.
virtual bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result)
Sample a point that is included in the body using a given random number generator.
#define CONSOLE_BRIDGE_logError(fmt,...)
void correctVertexOrderFromPlanes()
bool isPointInsidePlanes(const Eigen::Vector3d &point) const
Check if a point is inside a set of planes that make up a convex mesh.
const EigenSTL::vector_Vector3d & getScaledVertices() const
virtual void computeBoundingSphere(BoundingSphere &sphere) const =0
Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for...
Definition of a triangle mesh By convention the "center" of the shape is at the origin. For a mesh this implies that the AABB of the mesh is centered at the origin. Some methods may not work with arbitrary meshes whose AABB is not centered at the origin.
Definition of a box Aligned with the XYZ axes.
void setDimensions(const shapes::Shape *shape)
Set the dimensions of the body (from corresponding shape)
virtual void computeBoundingSphere(BoundingSphere &sphere) const
Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for...
virtual double computeVolume() const
Compute the volume of the body. This method includes changes induced by scaling and padding...
virtual double computeVolume() const
Compute the volume of the body. This method includes changes induced by scaling and padding...
void addBody(Body *body)
Add a body.
virtual bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const
Check if a point is inside the body.
void computeBoundingSphere(const std::vector< const Body * > &bodies, BoundingSphere &mergedSphere)
Compute the bounding sphere for a set of bodies and store the resulting sphere in mergedSphere...
const Body * getBody(unsigned int i) const
Get the ith body in the vector.
A body is a shape + its pose. Point inclusion, ray intersection can be tested, volumes and bounding s...
void setPadding(double padd)
If constant padding should be added to the body, this method sets the padding. Default is 0...
double scale_
The scale that was set for this body.
void computeScaledVerticesFromPlaneProjections()
Project the original vertex to the scaled and padded planes and average.
std::size_t getCount() const
Get the number of bodies in this vector.
virtual std::vector< double > getDimensions() const
Get the radius of the sphere.
This set of classes allows quickly detecting whether a given point is inside an object or not...
virtual BodyPtr cloneAt(const Eigen::Affine3d &pose, double padding, double scale) const
Get a clone of this body, but one that is located at the pose pose and has possibly different passing...