bodies.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
39 
41 
42 extern "C" {
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>
52 #else
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>
59 #include <qhull/io.h>
60 #include <qhull/stat.h>
61 #endif
62 }
63 
64 #include <boost/math/constants/constants.hpp>
65 #include <limits>
66 #include <cstdio>
67 #include <algorithm>
68 #include <Eigen/Geometry>
69 
70 namespace bodies
71 {
72 namespace detail
73 {
74 static const double ZERO = 1e-9;
75 
78 static inline double distanceSQR(const Eigen::Vector3d& p, const Eigen::Vector3d& origin, const Eigen::Vector3d& dir)
79 {
80  Eigen::Vector3d a = p - origin;
81  double d = dir.normalized().dot(a);
82  return a.squaredNorm() - d * d;
83 }
84 
85 // temp structure for intersection points (used for ordering them)
86 struct intersc
87 {
88  intersc(const Eigen::Vector3d& _pt, const double _tm) : pt(_pt), time(_tm)
89  {
90  }
91 
92  Eigen::Vector3d pt;
93  double time;
94 
95  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
96 };
97 
98 // define order on intersection points
100 {
101  bool operator()(const intersc& a, const intersc& b) const
102  {
103  return a.time < b.time;
104  }
105 };
106 }
107 }
108 
110 {
111  useDimensions(shape);
112  updateInternalData();
113 }
114 
116  Eigen::Vector3d& result)
117 {
118  BoundingSphere bs;
120  for (unsigned int i = 0; i < max_attempts; ++i)
121  {
122  result = Eigen::Vector3d(rng.uniformReal(bs.center.x() - bs.radius, bs.center.x() + bs.radius),
123  rng.uniformReal(bs.center.y() - bs.radius, bs.center.y() + bs.radius),
124  rng.uniformReal(bs.center.z() - bs.radius, bs.center.z() + bs.radius));
125  if (containsPoint(result))
126  return true;
127  }
128  return false;
129 }
130 
131 bool bodies::Sphere::containsPoint(const Eigen::Vector3d& p, bool verbose) const
132 {
133  return (center_ - p).squaredNorm() < radius2_;
134 }
135 
136 void bodies::Sphere::useDimensions(const shapes::Shape* shape) // radius
137 {
138  radius_ = static_cast<const shapes::Sphere*>(shape)->radius;
139 }
140 
141 std::vector<double> bodies::Sphere::getDimensions() const
142 {
143  std::vector<double> d(1, radius_);
144  return d;
145 }
146 
148 {
149  radiusU_ = radius_ * scale_ + padding_;
150  radius2_ = radiusU_ * radiusU_;
151  center_ = pose_.translation();
152 }
153 
154 std::shared_ptr<bodies::Body> bodies::Sphere::cloneAt(const Eigen::Affine3d& pose, double padding, double scale) const
155 {
156  Sphere* s = new Sphere();
157  s->radius_ = radius_;
158  s->padding_ = padding;
159  s->scale_ = scale;
160  s->pose_ = pose;
161  s->updateInternalData();
162  return std::shared_ptr<Body>(s);
163 }
164 
166 {
167  return 4.0 * boost::math::constants::pi<double>() * radiusU_ * radiusU_ * radiusU_ / 3.0;
168 }
169 
171 {
172  sphere.center = center_;
173  sphere.radius = radiusU_;
174 }
175 
177 {
178  cylinder.pose = pose_;
179  cylinder.radius = radiusU_;
180  cylinder.length = radiusU_;
181 }
182 
184  Eigen::Vector3d& result)
185 {
186  for (unsigned int i = 0; i < max_attempts; ++i)
187  {
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_;
194  // we are sampling in a box; the probability of success after 20 attempts is 99.99996% given the ratio of box volume
195  // to sphere volume
196  for (int j = 0; j < 20; ++j)
197  {
198  result = Eigen::Vector3d(rng.uniformReal(minX, maxX), rng.uniformReal(minY, maxY), rng.uniformReal(minZ, maxZ));
199  if (containsPoint(result))
200  return true;
201  }
202  }
203  return false;
204 }
205 
206 bool bodies::Sphere::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
207  EigenSTL::vector_Vector3d* intersections, unsigned int count) const
208 {
209  if (detail::distanceSQR(center_, origin, dir) > radius2_)
210  return false;
211 
212  bool result = false;
213 
214  Eigen::Vector3d cp = origin - center_;
215  double dpcpv = cp.dot(dir);
216 
217  Eigen::Vector3d w = cp - dpcpv * dir;
218  Eigen::Vector3d Q = center_ + w;
219  double x = radius2_ - w.squaredNorm();
220 
221  if (fabs(x) < detail::ZERO)
222  {
223  w = Q - origin;
224  double dpQv = w.dot(dir);
225  if (dpQv > detail::ZERO)
226  {
227  if (intersections)
228  intersections->push_back(Q);
229  result = true;
230  }
231  }
232  else if (x > 0.0)
233  {
234  x = sqrt(x);
235  w = dir * x;
236  Eigen::Vector3d A = Q - w;
237  Eigen::Vector3d B = Q + w;
238  w = A - origin;
239  double dpAv = w.dot(dir);
240  w = B - origin;
241  double dpBv = w.dot(dir);
242 
243  if (dpAv > detail::ZERO)
244  {
245  result = true;
246  if (intersections)
247  {
248  intersections->push_back(A);
249  if (count == 1)
250  return result;
251  }
252  }
253 
254  if (dpBv > detail::ZERO)
255  {
256  result = true;
257  if (intersections)
258  intersections->push_back(B);
259  }
260  }
261  return result;
262 }
263 
264 bool bodies::Cylinder::containsPoint(const Eigen::Vector3d& p, bool verbose) const
265 {
266  Eigen::Vector3d v = p - center_;
267  double pH = v.dot(normalH_);
268 
269  if (fabs(pH) > length2_)
270  return false;
271 
272  double pB1 = v.dot(normalB1_);
273  double remaining = radius2_ - pB1 * pB1;
274 
275  if (remaining < 0.0)
276  return false;
277  else
278  {
279  double pB2 = v.dot(normalB2_);
280  return pB2 * pB2 < remaining;
281  }
282 }
283 
284 void bodies::Cylinder::useDimensions(const shapes::Shape* shape) // (length, radius)
285 {
286  length_ = static_cast<const shapes::Cylinder*>(shape)->length;
287  radius_ = static_cast<const shapes::Cylinder*>(shape)->radius;
288 }
289 
290 std::vector<double> bodies::Cylinder::getDimensions() const
291 {
292  std::vector<double> d(2);
293  d[0] = radius_;
294  d[1] = length_;
295  return d;
296 }
297 
299 {
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_);
306 
307  Eigen::Matrix3d basis = pose_.rotation();
308  normalB1_ = basis.col(0);
309  normalB2_ = basis.col(1);
310  normalH_ = basis.col(2);
311 
312  double tmp = -normalH_.dot(center_);
313  d1_ = tmp + length2_;
314  d2_ = tmp - length2_;
315 }
316 
318  Eigen::Vector3d& result)
319 {
320  // sample a point on the base disc of the cylinder
321  double a = rng.uniformReal(-boost::math::constants::pi<double>(), boost::math::constants::pi<double>());
322  double r = rng.uniformReal(-radiusU_, radiusU_);
323  double x = cos(a) * r;
324  double y = sin(a) * r;
325 
326  // sample e height
327  double z = rng.uniformReal(-length2_, length2_);
328 
329  result = Eigen::Vector3d(x, y, z);
330  return true;
331 }
332 
333 std::shared_ptr<bodies::Body> bodies::Cylinder::cloneAt(const Eigen::Affine3d& pose, double padding, double scale) const
334 {
335  Cylinder* c = new Cylinder();
336  c->length_ = length_;
337  c->radius_ = radius_;
338  c->padding_ = padding;
339  c->scale_ = scale;
340  c->pose_ = pose;
341  c->updateInternalData();
342  return std::shared_ptr<Body>(c);
343 }
344 
346 {
347  return 2.0 * boost::math::constants::pi<double>() * radius2_ * length2_;
348 }
349 
351 {
352  sphere.center = center_;
353  sphere.radius = radiusB_;
354 }
355 
357 {
358  cylinder.pose = pose_;
359  cylinder.radius = radiusU_;
360  cylinder.length = scale_ * length_ + padding_;
361 }
362 
363 bool bodies::Cylinder::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
364  EigenSTL::vector_Vector3d* intersections, unsigned int count) const
365 {
366  if (detail::distanceSQR(center_, origin, dir) > radiusBSqr_)
367  return false;
368 
369  std::vector<detail::intersc> ipts;
370 
371  // intersect bases
372  double tmp = normalH_.dot(dir);
373  if (fabs(tmp) > detail::ZERO)
374  {
375  double tmp2 = -normalH_.dot(origin);
376  double t1 = (tmp2 - d1_) / tmp;
377 
378  if (t1 > 0.0)
379  {
380  Eigen::Vector3d p1(origin + dir * t1);
381  Eigen::Vector3d v1(p1 - center_);
382  v1 = v1 - normalH_.dot(v1) * normalH_;
383  if (v1.squaredNorm() < radius2_ + detail::ZERO)
384  {
385  if (intersections == NULL)
386  return true;
387 
388  detail::intersc ip(p1, t1);
389  ipts.push_back(ip);
390  }
391  }
392 
393  double t2 = (tmp2 - d2_) / tmp;
394  if (t2 > 0.0)
395  {
396  Eigen::Vector3d p2(origin + dir * t2);
397  Eigen::Vector3d v2(p2 - center_);
398  v2 = v2 - normalH_.dot(v2) * normalH_;
399  if (v2.squaredNorm() < radius2_ + detail::ZERO)
400  {
401  if (intersections == NULL)
402  return true;
403 
404  detail::intersc ip(p2, t2);
405  ipts.push_back(ip);
406  }
407  }
408  }
409 
410  if (ipts.size() < 2)
411  {
412  // intersect with infinite cylinder
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;
419  if (d > 0.0 && fabs(a) > detail::ZERO)
420  {
421  d = sqrt(d);
422  double e = -a * 2.0;
423  double t1 = (b + d) / e;
424  double t2 = (b - d) / e;
425 
426  if (t1 > 0.0)
427  {
428  Eigen::Vector3d p1(origin + dir * t1);
429  Eigen::Vector3d v1(center_ - p1);
430 
431  if (fabs(normalH_.dot(v1)) < length2_ + detail::ZERO)
432  {
433  if (intersections == NULL)
434  return true;
435 
436  detail::intersc ip(p1, t1);
437  ipts.push_back(ip);
438  }
439  }
440 
441  if (t2 > 0.0)
442  {
443  Eigen::Vector3d p2(origin + dir * t2);
444  Eigen::Vector3d v2(center_ - p2);
445 
446  if (fabs(normalH_.dot(v2)) < length2_ + detail::ZERO)
447  {
448  if (intersections == NULL)
449  return true;
450  detail::intersc ip(p2, t2);
451  ipts.push_back(ip);
452  }
453  }
454  }
455  }
456 
457  if (ipts.empty())
458  return false;
459 
460  std::sort(ipts.begin(), ipts.end(), detail::interscOrder());
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);
464 
465  return true;
466 }
467 
469  Eigen::Vector3d& result)
470 {
471  result = pose_ * Eigen::Vector3d(rng.uniformReal(-length2_, length2_), rng.uniformReal(-width2_, width2_),
472  rng.uniformReal(-height2_, height2_));
473  return true;
474 }
475 
476 bool bodies::Box::containsPoint(const Eigen::Vector3d& p, bool verbose) const
477 {
478  Eigen::Vector3d v = p - center_;
479  double pL = v.dot(normalL_);
480  if (fabs(pL) > length2_)
481  return false;
482 
483  double pW = v.dot(normalW_);
484  if (fabs(pW) > width2_)
485  return false;
486 
487  double pH = v.dot(normalH_);
488  if (fabs(pH) > height2_)
489  return false;
490 
491  return true;
492 }
493 
494 void bodies::Box::useDimensions(const shapes::Shape* shape) // (x, y, z) = (length, width, height)
495 {
496  const double* size = static_cast<const shapes::Box*>(shape)->size;
497  length_ = size[0];
498  width_ = size[1];
499  height_ = size[2];
500 }
501 
502 std::vector<double> bodies::Box::getDimensions() const
503 {
504  std::vector<double> d(3);
505  d[0] = length_;
506  d[1] = width_;
507  d[2] = height_;
508  return d;
509 }
510 
512 {
513  double s2 = scale_ / 2.0;
514  length2_ = length_ * s2 + padding_;
515  width2_ = width_ * s2 + padding_;
516  height2_ = height_ * s2 + padding_;
517 
518  center_ = pose_.translation();
519 
520  radius2_ = length2_ * length2_ + width2_ * width2_ + height2_ * height2_;
521  radiusB_ = sqrt(radius2_);
522 
523  Eigen::Matrix3d basis = pose_.rotation();
524  normalL_ = basis.col(0);
525  normalW_ = basis.col(1);
526  normalH_ = basis.col(2);
527 
528  const Eigen::Vector3d tmp(normalL_ * length2_ + normalW_ * width2_ + normalH_ * height2_);
529  corner1_ = center_ - tmp;
530  corner2_ = center_ + tmp;
531 }
532 
533 std::shared_ptr<bodies::Body> bodies::Box::cloneAt(const Eigen::Affine3d& pose, double padding, double scale) const
534 {
535  Box* b = new Box();
536  b->length_ = length_;
537  b->width_ = width_;
538  b->height_ = height_;
539  b->padding_ = padding;
540  b->scale_ = scale;
541  b->pose_ = pose;
542  b->updateInternalData();
543  return std::shared_ptr<Body>(b);
544 }
545 
547 {
548  return 8.0 * length2_ * width2_ * height2_;
549 }
550 
552 {
553  sphere.center = center_;
554  sphere.radius = radiusB_;
555 }
556 
558 {
559  double a, b;
560 
561  if (length2_ > width2_ && length2_ > height2_)
562  {
563  cylinder.length = length2_ * 2.0;
564  a = width2_;
565  b = height2_;
566  Eigen::Affine3d rot(Eigen::AngleAxisd(90.0f * (M_PI / 180.0f), Eigen::Vector3d::UnitY()));
567  cylinder.pose = pose_ * rot;
568  }
569  else if (width2_ > height2_)
570  {
571  cylinder.length = width2_ * 2.0;
572  a = height2_;
573  b = length2_;
574  cylinder.radius = sqrt(height2_ * height2_ + length2_ * length2_);
575  Eigen::Affine3d rot(Eigen::AngleAxisd(90.0f * (M_PI / 180.0f), Eigen::Vector3d::UnitX()));
576  cylinder.pose = pose_ * rot;
577  }
578  else
579  {
580  cylinder.length = height2_ * 2.0;
581  a = width2_;
582  b = length2_;
583  cylinder.pose = pose_;
584  }
585  cylinder.radius = sqrt(a * a + b * b);
586 }
587 
588 bool bodies::Box::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
589  EigenSTL::vector_Vector3d* intersections, unsigned int count) const
590 {
591  // Brian Smits. Efficient bounding box intersection. Ray tracing news 15(1), 2002
592  float tmin, tmax, tymin, tymax, tzmin, tzmax;
593  float divx, divy, divz;
594  divx = 1 / dir.x();
595  if (divx >= 0)
596  {
597  tmin = (corner1_.x() - origin.x()) * divx;
598  tmax = (corner2_.x() - origin.x()) * divx;
599  }
600  else
601  {
602  tmax = (corner1_.x() - origin.x()) * divx;
603  tmin = (corner2_.x() - origin.x()) * divx;
604  }
605 
606  divy = 1 / dir.y();
607  if (dir.y() >= 0)
608  {
609  tymin = (corner1_.y() - origin.y()) * divy;
610  tymax = (corner2_.y() - origin.y()) * divy;
611  }
612  else
613  {
614  tymax = (corner1_.y() - origin.y()) * divy;
615  tymin = (corner2_.y() - origin.y()) * divy;
616  }
617 
618  if ((tmin > tymax || tymin > tmax))
619  return false;
620 
621  if (tymin > tmin)
622  tmin = tymin;
623  if (tymax < tmax)
624  tmax = tymax;
625 
626  divz = 1 / dir.z();
627  if (dir.z() >= 0)
628  {
629  tzmin = (corner1_.z() - origin.z()) * divz;
630  tzmax = (corner2_.z() - origin.z()) * divz;
631  }
632  else
633  {
634  tzmax = (corner1_.z() - origin.z()) * divz;
635  tzmin = (corner2_.z() - origin.z()) * divz;
636  }
637 
638  if ((tmin > tzmax || tzmin > tmax))
639  return false;
640 
641  if (tzmin > tmin)
642  tmin = tzmin;
643  if (tzmax < tmax)
644  tmax = tzmax;
645 
646  if (tmax < 0)
647  return false;
648 
649  if (intersections)
650  {
651  if (tmax - tmin > detail::ZERO)
652  {
653  intersections->push_back(tmin * dir + origin);
654  if (count > 1)
655  intersections->push_back(tmax * dir + origin);
656  }
657  else
658  intersections->push_back(tmax * dir + origin);
659  }
660 
661  return true;
662 }
663 
664 bool bodies::ConvexMesh::containsPoint(const Eigen::Vector3d& p, bool verbose) const
665 {
666  if (!mesh_data_)
667  return false;
668  if (bounding_box_.containsPoint(p))
669  {
670  Eigen::Vector3d ip(i_pose_ * p);
671  ip = mesh_data_->mesh_center_ + (ip - mesh_data_->mesh_center_) / scale_;
672  return isPointInsidePlanes(ip);
673  }
674  else
675  return false;
676 }
677 
679 {
680  for (unsigned int i = 0; i < mesh_data_->triangles_.size(); i += 3)
681  {
682  Eigen::Vector3d d1 =
683  mesh_data_->vertices_[mesh_data_->triangles_[i]] - mesh_data_->vertices_[mesh_data_->triangles_[i + 1]];
684  Eigen::Vector3d d2 =
685  mesh_data_->vertices_[mesh_data_->triangles_[i]] - mesh_data_->vertices_[mesh_data_->triangles_[i + 2]];
686  // expected computed normal from triangle vertex order
687  Eigen::Vector3d tri_normal = d1.cross(d2);
688  tri_normal.normalize();
689  // actual plane normal
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;
694  if (!same_dir)
695  {
696  std::swap(mesh_data_->triangles_[i], mesh_data_->triangles_[i + 1]);
697  }
698  }
699 }
700 
702 {
703  mesh_data_.reset(new MeshData());
704  const shapes::Mesh* mesh = static_cast<const shapes::Mesh*>(shape);
705 
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();
710 
711  for (unsigned int i = 0; i < mesh->vertex_count; ++i)
712  {
713  double vx = mesh->vertices[3 * i];
714  double vy = mesh->vertices[3 * i + 1];
715  double vz = mesh->vertices[3 * i + 2];
716 
717  if (maxX < vx)
718  maxX = vx;
719  if (maxY < vy)
720  maxY = vy;
721  if (maxZ < vz)
722  maxZ = vz;
723 
724  if (minX > vx)
725  minX = vx;
726  if (minY > vy)
727  minY = vy;
728  if (minZ > vz)
729  minZ = vz;
730  }
731 
732  if (maxX < minX)
733  maxX = minX = 0.0;
734  if (maxY < minY)
735  maxY = minY = 0.0;
736  if (maxZ < minZ)
737  maxZ = minZ = 0.0;
738 
739  mesh_data_->box_size_ = Eigen::Vector3d(maxX - minX, maxY - minY, maxZ - minZ);
740 
741  mesh_data_->box_offset_ = Eigen::Vector3d((minX + maxX) / 2.0, (minY + maxY) / 2.0, (minZ + maxZ) / 2.0);
742 
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();
748 
749  double xdim = maxX - minX;
750  double ydim = maxY - minY;
751  double zdim = maxZ - minZ;
752 
753  double pose1;
754  double pose2;
755 
756  unsigned int off1;
757  unsigned int off2;
758 
759  /* compute bounding cylinder */
760  double cyl_length;
761  double maxdist = -std::numeric_limits<double>::infinity();
762  if (xdim > ydim && xdim > zdim)
763  {
764  off1 = 1;
765  off2 = 2;
766  pose1 = mesh_data_->box_offset_.y();
767  pose2 = mesh_data_->box_offset_.z();
768  cyl_length = xdim;
769  }
770  else if (ydim > zdim)
771  {
772  off1 = 0;
773  off2 = 2;
774  pose1 = mesh_data_->box_offset_.x();
775  pose2 = mesh_data_->box_offset_.z();
776  cyl_length = ydim;
777  }
778  else
779  {
780  off1 = 0;
781  off2 = 1;
782  pose1 = mesh_data_->box_offset_.x();
783  pose2 = mesh_data_->box_offset_.y();
784  cyl_length = zdim;
785  }
786 
787  /* compute convex hull */
788  coordT* points = (coordT*)calloc(mesh->vertex_count * 3, sizeof(coordT));
789  for (unsigned int i = 0; i < mesh->vertex_count; ++i)
790  {
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];
794 
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)));
798  if (dist > maxdist)
799  maxdist = dist;
800  }
801  mesh_data_->bounding_cylinder_.radius = maxdist;
802  mesh_data_->bounding_cylinder_.length = cyl_length;
803 
804  static FILE* null = fopen("/dev/null", "w");
805 
806  char flags[] = "qhull Tv Qt";
807  int exitcode = qh_new_qhull(3, mesh->vertex_count, points, true, flags, null, null);
808 
809  if (exitcode != 0)
810  {
811  CONSOLE_BRIDGE_logWarn("Convex hull creation failed");
812  qh_freeqhull(!qh_ALL);
813  int curlong, totlong;
814  qh_memfreeshort(&curlong, &totlong);
815  return;
816  }
817 
818  int num_facets = qh num_facets;
819 
820  int num_vertices = qh num_vertices;
821  mesh_data_->vertices_.reserve(num_vertices);
822  Eigen::Vector3d sum(0, 0, 0);
823 
824  // necessary for FORALLvertices
825  std::map<unsigned int, unsigned int> qhull_vertex_table;
826  vertexT* vertex;
827  FORALLvertices
828  {
829  Eigen::Vector3d vert(vertex->point[0], vertex->point[1], vertex->point[2]);
830  qhull_vertex_table[vertex->id] = mesh_data_->vertices_.size();
831  sum += vert;
832  mesh_data_->vertices_.push_back(vert);
833  }
834 
835  mesh_data_->mesh_center_ = sum / (double)(num_vertices);
836  for (unsigned int j = 0; j < mesh_data_->vertices_.size(); ++j)
837  {
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;
841  }
842 
843  mesh_data_->mesh_radiusB_ = sqrt(mesh_data_->mesh_radiusB_);
844  mesh_data_->triangles_.reserve(num_facets);
845 
846  // neccessary for qhull macro
847  facetT* facet;
848  FORALLfacets
849  {
850  Eigen::Vector4f planeEquation(facet->normal[0], facet->normal[1], facet->normal[2], facet->offset);
851  if (!mesh_data_->planes_.empty())
852  {
853  // filter equal planes - assuming same ones follow each other
854  if ((planeEquation - mesh_data_->planes_.back()).cwiseAbs().maxCoeff() > 1e-6) // max diff to last
855  mesh_data_->planes_.push_back(planeEquation);
856  }
857  else
858  {
859  mesh_data_->planes_.push_back(planeEquation);
860  }
861 
862  // Needed by FOREACHvertex_i_
863  int vertex_n, vertex_i;
864  FOREACHvertex_i_((*facet).vertices)
865  {
866  mesh_data_->triangles_.push_back(qhull_vertex_table[vertex->id]);
867  }
868 
869  mesh_data_->plane_for_triangle_[(mesh_data_->triangles_.size() - 1) / 3] = mesh_data_->planes_.size() - 1;
870  }
871  qh_freeqhull(!qh_ALL);
872  int curlong, totlong;
873  qh_memfreeshort(&curlong, &totlong);
874 }
875 
876 std::vector<double> bodies::ConvexMesh::getDimensions() const
877 {
878  return std::vector<double>();
879 }
880 
882 {
883  // compute the scaled vertices, if needed
884  if (padding_ == 0.0 && scale_ == 1.0)
885  {
886  scaled_vertices_ = &mesh_data_->vertices_;
887  return;
888  }
889 
890  if (!scaled_vertices_storage_)
891  scaled_vertices_storage_.reset(new EigenSTL::vector_Vector3d());
892  scaled_vertices_ = scaled_vertices_storage_.get();
893  scaled_vertices_storage_->resize(mesh_data_->vertices_.size());
894  // project vertices along the vertex - center line to the scaled and padded plane
895  // take the average of all tri's planes around that vertex as the result
896  // is not unique
897 
898  // First figure out, which tris are connected to each vertex
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)
901  {
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);
905  }
906 
907  for (unsigned int i = 0; i < mesh_data_->vertices_.size(); ++i)
908  {
909  Eigen::Vector3d v(mesh_data_->vertices_[i] - mesh_data_->mesh_center_);
910  EigenSTL::vector_Vector3d projected_vertices;
911  for (unsigned int t = 0; t < vertex_to_tris[i].size(); ++t)
912  {
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_;
917 
918  // intersect vert - center with scaled/padded plane equation
919  double denom = v.dot(plane_normal);
920  if (fabs(denom) < 1e-3)
921  continue;
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);
925  }
926  if (projected_vertices.empty())
927  {
928  double l = v.norm();
929  scaled_vertices_storage_->at(i) =
930  mesh_data_->mesh_center_ + v * (scale_ + (l > detail::ZERO ? padding_ / l : 0.0));
931  }
932  else
933  {
934  Eigen::Vector3d sum(0, 0, 0);
935  for (unsigned int v = 0; v < projected_vertices.size(); ++v)
936  {
937  sum += projected_vertices[v];
938  }
939  sum /= projected_vertices.size();
940  scaled_vertices_storage_->at(i) = sum;
941  }
942  }
943 }
944 
946 {
947  if (!mesh_data_)
948  return;
949  Eigen::Affine3d pose = pose_;
950  pose.translation() = Eigen::Vector3d(pose_ * mesh_data_->box_offset_);
951 
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_);
958 
959  i_pose_ = pose_.inverse();
960  center_ = pose_ * mesh_data_->mesh_center_;
961  radiusB_ = mesh_data_->mesh_radiusB_ * scale_ + padding_;
962  radiusBSqr_ = radiusB_ * radiusB_;
963 
964  // compute the scaled vertices, if needed
965  if (padding_ == 0.0 && scale_ == 1.0)
966  scaled_vertices_ = &mesh_data_->vertices_;
967  else
968  {
969  if (!scaled_vertices_storage_)
970  scaled_vertices_storage_.reset(new EigenSTL::vector_Vector3d());
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)
974  {
975  Eigen::Vector3d v(mesh_data_->vertices_[i] - mesh_data_->mesh_center_);
976  double l = v.norm();
977  scaled_vertices_storage_->at(i) =
978  mesh_data_->mesh_center_ + v * (scale_ + (l > detail::ZERO ? padding_ / l : 0.0));
979  }
980  }
981 }
982 const std::vector<unsigned int>& bodies::ConvexMesh::getTriangles() const
983 {
984  static const std::vector<unsigned int> empty;
985  return mesh_data_ ? mesh_data_->triangles_ : empty;
986 }
987 
989 {
990  static const EigenSTL::vector_Vector3d empty;
991  return mesh_data_ ? mesh_data_->vertices_ : empty;
992 }
993 
995 {
996  return scaled_vertices_ ? *scaled_vertices_ : getVertices();
997 }
998 
999 std::shared_ptr<bodies::Body> bodies::ConvexMesh::cloneAt(const Eigen::Affine3d& pose, double padding,
1000  double scale) const
1001 {
1002  ConvexMesh* m = new ConvexMesh();
1003  m->mesh_data_ = mesh_data_;
1004  m->padding_ = padding;
1005  m->scale_ = scale;
1006  m->pose_ = pose;
1007  m->updateInternalData();
1008  return std::shared_ptr<Body>(m);
1009 }
1010 
1012 {
1013  sphere.center = center_;
1014  sphere.radius = radiusB_;
1015 }
1016 
1018 {
1019  cylinder.length = mesh_data_ ? mesh_data_->bounding_cylinder_.length : 0.0;
1020  cylinder.radius = mesh_data_ ? mesh_data_->bounding_cylinder_.radius : 0.0;
1021  // need to do rotation correctly to get pose, which bounding box does
1022  BoundingCylinder cyl;
1023  bounding_box_.computeBoundingCylinder(cyl);
1024  cylinder.pose = cyl.pose;
1025 }
1026 
1027 bool bodies::ConvexMesh::isPointInsidePlanes(const Eigen::Vector3d& point) const
1028 {
1029  unsigned int numplanes = mesh_data_->planes_.size();
1030  for (unsigned int i = 0; i < numplanes; ++i)
1031  {
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;
1035  if (dist > 0.0)
1036  return false;
1037  }
1038  return true;
1039 }
1040 
1041 unsigned int bodies::ConvexMesh::countVerticesBehindPlane(const Eigen::Vector4f& planeNormal) const
1042 {
1043  unsigned int numvertices = mesh_data_->vertices_.size();
1044  unsigned int result = 0;
1045  for (unsigned int i = 0; i < numvertices; ++i)
1046  {
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;
1049  if (dist > 0.0)
1050  result++;
1051  }
1052  return result;
1053 }
1054 
1056 {
1057  double volume = 0.0;
1058  if (mesh_data_)
1059  for (unsigned int i = 0; i < mesh_data_->triangles_.size() / 3; ++i)
1060  {
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();
1066  }
1067  return fabs(volume) / 6.0;
1068 }
1069 
1070 bool bodies::ConvexMesh::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
1071  EigenSTL::vector_Vector3d* intersections, unsigned int count) const
1072 {
1073  if (!mesh_data_)
1074  return false;
1075  if (detail::distanceSQR(center_, origin, dir) > radiusBSqr_)
1076  return false;
1077  if (!bounding_box_.intersectsRay(origin, dir))
1078  return false;
1079 
1080  // transform the ray into the coordinate frame of the mesh
1081  Eigen::Vector3d orig(i_pose_ * origin);
1082  Eigen::Vector3d dr(i_pose_ * dir);
1083 
1084  std::vector<detail::intersc> ipts;
1085 
1086  bool result = false;
1087 
1088  // for each triangle
1089  const unsigned int nt = mesh_data_->triangles_.size() / 3;
1090  for (unsigned int i = 0; i < nt; ++i)
1091  {
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());
1095 
1096  double tmp = vec.dot(dr);
1097  if (fabs(tmp) > detail::ZERO)
1098  {
1099  double t = -(vec.dot(orig) + mesh_data_->planes_[mesh_data_->plane_for_triangle_[i]].w()) / tmp;
1100  if (t > 0.0)
1101  {
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];
1106 
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);
1110 
1111  Eigen::Vector3d cb(c - b);
1112  Eigen::Vector3d ab(a - b);
1113 
1114  // intersection of the plane defined by the triangle and the ray
1115  Eigen::Vector3d P(orig + dr * t);
1116 
1117  // check if it is inside the triangle
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)
1122  continue;
1123 
1124  Eigen::Vector3d ca(c - a);
1125  Eigen::Vector3d pa(P - a);
1126  Eigen::Vector3d ba(-ab);
1127 
1128  c1 = ca.cross(pa);
1129  c2 = ca.cross(ba);
1130  if (c1.dot(c2) < 0.0)
1131  continue;
1132 
1133  c1 = ba.cross(pa);
1134  c2 = ba.cross(ca);
1135 
1136  if (c1.dot(c2) < 0.0)
1137  continue;
1138 
1139  result = true;
1140  if (intersections)
1141  {
1142  detail::intersc ip(origin + dir * t, t);
1143  ipts.push_back(ip);
1144  }
1145  else
1146  break;
1147  }
1148  }
1149  }
1150 
1151  if (intersections)
1152  {
1153  std::sort(ipts.begin(), ipts.end(), detail::interscOrder());
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);
1157  }
1158 
1159  return result;
1160 }
1161 
1163 {
1164 }
1165 
1166 bodies::BodyVector::BodyVector(const std::vector<shapes::Shape*>& shapes, const EigenSTL::vector_Affine3d& poses,
1167  double padding)
1168 {
1169  for (unsigned int i = 0; i < shapes.size(); i++)
1170  addBody(shapes[i], poses[i], padding);
1171 }
1172 
1174 {
1175  clear();
1176 }
1177 
1179 {
1180  for (unsigned int i = 0; i < bodies_.size(); i++)
1181  delete bodies_[i];
1182  bodies_.clear();
1183 }
1184 
1186 {
1187  bodies_.push_back(body);
1188  BoundingSphere sphere;
1189  body->computeBoundingSphere(sphere);
1190 }
1191 
1192 void bodies::BodyVector::addBody(const shapes::Shape* shape, const Eigen::Affine3d& pose, double padding)
1193 {
1195  body->setPose(pose);
1196  body->setPadding(padding);
1197  addBody(body);
1198 }
1199 
1200 std::size_t bodies::BodyVector::getCount() const
1201 {
1202  return bodies_.size();
1203 }
1204 
1205 void bodies::BodyVector::setPose(unsigned int i, const Eigen::Affine3d& pose)
1206 {
1207  if (i >= bodies_.size())
1208  {
1209  CONSOLE_BRIDGE_logError("There is no body at index %u", i);
1210  return;
1211  }
1212 
1213  bodies_[i]->setPose(pose);
1214 }
1215 
1216 const bodies::Body* bodies::BodyVector::getBody(unsigned int i) const
1217 {
1218  if (i >= bodies_.size())
1219  {
1220  CONSOLE_BRIDGE_logError("There is no body at index %u", i);
1221  return NULL;
1222  }
1223  else
1224  return bodies_[i];
1225 }
1226 
1227 bool bodies::BodyVector::containsPoint(const Eigen::Vector3d& p, std::size_t& index, bool verbose) const
1228 {
1229  for (std::size_t i = 0; i < bodies_.size(); ++i)
1230  if (bodies_[i]->containsPoint(p, verbose))
1231  {
1232  index = i;
1233  return true;
1234  }
1235  return false;
1236 }
1237 
1238 bool bodies::BodyVector::containsPoint(const Eigen::Vector3d& p, bool verbose) const
1239 {
1240  std::size_t dummy;
1241  return containsPoint(p, dummy, verbose);
1242 }
1243 
1244 bool bodies::BodyVector::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, std::size_t& index,
1245  EigenSTL::vector_Vector3d* intersections, unsigned int count) const
1246 {
1247  for (std::size_t i = 0; i < bodies_.size(); ++i)
1248  if (bodies_[i]->intersectsRay(origin, dir, intersections, count))
1249  {
1250  index = i;
1251  return true;
1252  }
1253  return false;
1254 }
#define CONSOLE_BRIDGE_logWarn(fmt,...)
d
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.
Definition: bodies.h:135
virtual void updateInternalData()
This function is called every time a change to the body is made, so that intermediate values stored f...
Definition: bodies.cpp:298
const std::vector< unsigned int > & getTriangles() const
Definition: bodies.cpp:982
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...
Definition: bodies.cpp:588
virtual void useDimensions(const shapes::Shape *shape)
Depending on the shape, this function copies the relevant data to the body.
Definition: bodies.cpp:136
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...
Definition: bodies.cpp:206
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.
Definition: shapes.h:127
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
f
virtual std::vector< double > getDimensions() const
Returns an empty vector.
Definition: bodies.cpp:876
virtual bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const
Check if a point is inside the body.
Definition: bodies.cpp:476
Definition of a cylinder.
Definition: bodies.h:277
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.
Definition: bodies.cpp:317
virtual void computeBoundingSphere(BoundingSphere &sphere) const
Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for...
Definition: bodies.cpp:1011
virtual void useDimensions(const shapes::Shape *shape)
Depending on the shape, this function copies the relevant data to the body.
Definition: bodies.cpp:701
Definition of a sphere.
Definition: bodies.h:228
double radius_
Definition: bodies.h:265
virtual std::vector< double > getDimensions() const
Get the length & width & height (x, y, z) of the box.
Definition: bodies.cpp:502
virtual void computeBoundingSphere(BoundingSphere &sphere) const
Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for...
Definition: bodies.cpp:551
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...
Definition: bodies.cpp:1070
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.
Definition: bodies.cpp:468
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 &#39;dir&#39; to be normaliz...
Definition: bodies.cpp:78
intersc(const Eigen::Vector3d &_pt, const double _tm)
Definition: bodies.cpp:88
void setPose(unsigned int i, const Eigen::Affine3d &pose)
Set the pose of a particular body in the vector of bodies.
Definition: bodies.cpp:1205
std::shared_ptr< MeshData > mesh_data_
Definition: bodies.h:465
virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const
Compute the bounding cylinder for the body, in its current pose. Scaling and padding are accounted fo...
Definition: bodies.cpp:557
virtual bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const
Check if a point is inside the body.
Definition: bodies.cpp:264
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 ...
Definition: bodies.cpp:1244
virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const
Compute the bounding cylinder for the body, in its current pose. Scaling and padding are accounted fo...
Definition: bodies.cpp:1017
#define M_PI
bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const
Check if any body in the vector contains the input point.
Definition: bodies.cpp:1238
virtual bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const
Check if a point is inside the body.
Definition: bodies.cpp:131
Eigen::Affine3d pose
Definition: bodies.h:70
virtual void computeBoundingSphere(BoundingSphere &sphere) const
Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for...
Definition: bodies.cpp:350
virtual void useDimensions(const shapes::Shape *shape)
Depending on the shape, this function copies the relevant data to the body.
Definition: bodies.cpp:494
double length_
Definition: bodies.h:314
Eigen::Vector3d center
Definition: bodies.h:61
double y
bool operator()(const intersc &a, const intersc &b) const
Definition: bodies.cpp:101
Definition of a sphere that bounds another object.
Definition: bodies.h:59
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...
Definition: bodies.cpp:999
virtual double computeVolume() const
Compute the volume of the body. This method includes changes induced by scaling and padding...
Definition: bodies.cpp:165
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...
Definition: bodies.cpp:333
unsigned int vertex_count
The number of available vertices.
Definition: shapes.h:222
double width_
Definition: bodies.h:374
unsigned int index
A basic definition of a shape. Shapes are considered centered at origin.
Definition: shapes.h:77
virtual void updateInternalData()
This function is called every time a change to the body is made, so that intermediate values stored f...
Definition: bodies.cpp:511
virtual double computeVolume() const
Compute the volume of the body. This method includes changes induced by scaling and padding...
Definition: bodies.cpp:546
Definition of a convex mesh. Convex hull is computed for a given shape::Mesh.
Definition: bodies.h:397
unsigned int countVerticesBehindPlane(const Eigen::Vector4f &planeNormal) const
(Used mainly for debugging) Count the number of vertices behind a plane
Definition: bodies.cpp:1041
virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const
Compute the bounding cylinder for the body, in its current pose. Scaling and padding are accounted fo...
Definition: bodies.cpp:356
const EigenSTL::vector_Vector3d & getVertices() const
Definition: bodies.cpp:988
virtual std::vector< double > getDimensions() const
Get the radius & length of the cylinder.
Definition: bodies.cpp:290
Definition of a box.
Definition: bodies.h:336
Definition of a cylinder.
Definition: bodies.h:68
Body * createBodyFromShape(const shapes::Shape *shape)
Create a body from a given shape.
Eigen::Affine3d pose_
The location of the body (position and orientation)
Definition: bodies.h:221
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...
Definition: bodies.cpp:533
virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const
Compute the bounding cylinder for the body, in its current pose. Scaling and padding are accounted fo...
Definition: bodies.cpp:176
double z
double * vertices
The position for each vertex vertex k has values at index (3k, 3k+1, 3k+2) = (x,y,z)
Definition: shapes.h:226
double padding_
The scale that was set for this body.
Definition: bodies.h:215
double height_
Definition: bodies.h:375
void clear()
Clear all bodies from the vector.
Definition: bodies.cpp:1178
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.
Definition: bodies.cpp:183
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...
Definition: bodies.cpp:363
virtual void updateInternalData()
This function is called every time a change to the body is made, so that intermediate values stored f...
Definition: bodies.cpp:147
virtual void updateInternalData()
This function is called every time a change to the body is made, so that intermediate values stored f...
Definition: bodies.cpp:945
virtual void useDimensions(const shapes::Shape *shape)
Depending on the shape, this function copies the relevant data to the body.
Definition: bodies.cpp:284
Definition of a sphere.
Definition: shapes.h:106
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.
Definition: bodies.cpp:115
#define CONSOLE_BRIDGE_logError(fmt,...)
void correctVertexOrderFromPlanes()
Definition: bodies.cpp:678
bool isPointInsidePlanes(const Eigen::Vector3d &point) const
Check if a point is inside a set of planes that make up a convex mesh.
Definition: bodies.cpp:1027
const EigenSTL::vector_Vector3d & getScaledVertices() const
Definition: bodies.cpp:994
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: shapes.h:196
Definition of a box Aligned with the XYZ axes.
Definition: shapes.h:174
void setDimensions(const shapes::Shape *shape)
Set the dimensions of the body (from corresponding shape)
Definition: bodies.cpp:109
virtual void computeBoundingSphere(BoundingSphere &sphere) const
Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for...
Definition: bodies.cpp:170
virtual double computeVolume() const
Compute the volume of the body. This method includes changes induced by scaling and padding...
Definition: bodies.cpp:345
static const double ZERO
Definition: bodies.cpp:74
double radius_
Definition: bodies.h:315
virtual double computeVolume() const
Compute the volume of the body. This method includes changes induced by scaling and padding...
Definition: bodies.cpp:1055
void addBody(Body *body)
Add a body.
Definition: bodies.cpp:1185
double length_
Definition: bodies.h:373
double x
virtual bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const
Check if a point is inside the body.
Definition: bodies.cpp:664
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...
r
const Body * getBody(unsigned int i) const
Get the ith body in the vector.
Definition: bodies.cpp:1216
A body is a shape + its pose. Point inclusion, ray intersection can be tested, volumes and bounding s...
Definition: bodies.h:88
void setPadding(double padd)
If constant padding should be added to the body, this method sets the padding. Default is 0...
Definition: bodies.h:122
double scale_
The scale that was set for this body.
Definition: bodies.h:212
void computeScaledVerticesFromPlaneProjections()
Project the original vertex to the scaled and padded planes and average.
Definition: bodies.cpp:881
std::size_t getCount() const
Get the number of bodies in this vector.
Definition: bodies.cpp:1200
virtual std::vector< double > getDimensions() const
Get the radius of the sphere.
Definition: bodies.cpp:141
This set of classes allows quickly detecting whether a given point is inside an object or not...
Definition: bodies.h:56
Eigen::Vector3d pt
Definition: bodies.cpp:92
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...
Definition: bodies.cpp:154


geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Mon Jun 10 2019 13:22:04