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 
40 
41 #include <console_bridge/console.h>
42 
43 extern "C" {
44 #ifdef GEOMETRIC_SHAPES_HAVE_QHULL_2011
45 #include <libqhull/libqhull.h>
46 #include <libqhull/mem.h>
47 #include <libqhull/qset.h>
48 #include <libqhull/geom.h>
49 #include <libqhull/merge.h>
50 #include <libqhull/poly.h>
51 #include <libqhull/io.h>
52 #include <libqhull/stat.h>
53 #else
54 #include <qhull/qhull.h>
55 #include <qhull/mem.h>
56 #include <qhull/qset.h>
57 #include <qhull/geom.h>
58 #include <qhull/merge.h>
59 #include <qhull/poly.h>
60 #include <qhull/io.h>
61 #include <qhull/stat.h>
62 #endif
63 }
64 
65 #include <boost/math/constants/constants.hpp>
66 #include <limits>
67 #include <cstdio>
68 #include <cmath> // std::fmin, std::fmax
69 #include <algorithm>
70 #include <Eigen/Geometry>
71 #include <unordered_map>
72 #include <mutex>
73 
74 namespace bodies
75 {
76 namespace detail
77 {
78 static const double ZERO = 1e-9;
79 
82 static inline double distanceSQR(const Eigen::Vector3d& p, const Eigen::Vector3d& origin, const Eigen::Vector3d& dir)
83 {
84  Eigen::Vector3d a = p - origin;
85  double d = dir.normalized().dot(a);
86  return a.squaredNorm() - d * d;
87 }
88 
89 // temp structure for intersection points (used for ordering them)
90 struct intersc
91 {
92  intersc(const Eigen::Vector3d& _pt, const double _tm) : pt(_pt), time(_tm)
93  {
94  }
95 
96  Eigen::Vector3d pt;
97  double time;
98 
99  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
100 };
101 
102 // define order on intersection points
104 {
105  bool operator()(const intersc& a, const intersc& b) const
106  {
107  return a.time < b.time;
108  }
109 };
110 
117 void filterIntersections(std::vector<detail::intersc>& ipts, EigenSTL::vector_Vector3d* intersections,
118  const size_t count)
119 {
120  if (intersections == nullptr || ipts.empty())
121  return;
122 
123  std::sort(ipts.begin(), ipts.end(), interscOrder());
124  const auto n = count > 0 ? std::min<size_t>(count, ipts.size()) : ipts.size();
125 
126  for (const auto& p : ipts)
127  {
128  if (intersections->size() == n)
129  break;
130  if (!intersections->empty() && p.pt.isApprox(intersections->back(), ZERO))
131  continue;
132  intersections->push_back(p.pt);
133  }
134 }
135 
136 // HACK: The global map g_triangle_for_plane_ is needed for ABI compatibility with the melodic version of
137 // geometric_shapes; in newer releases, it should instead be added as a member to ConvexMesh::MeshData.
138 std::unordered_map<const ConvexMesh*, std::map<size_t, size_t>> g_triangle_for_plane_;
140 static std::map<size_t, size_t>& getTriangleForPlane(const ConvexMesh* mesh)
141 {
142  std::lock_guard<std::mutex> lock(g_triangle_for_plane_mutex);
143  auto it = g_triangle_for_plane_.find(mesh);
144  if (it == detail::g_triangle_for_plane_.end())
145  return detail::g_triangle_for_plane_.emplace(mesh, std::map<size_t, size_t>()).first->second;
146  else
147  return it->second;
148 }
149 } // namespace detail
150 
151 inline Eigen::Vector3d normalize(const Eigen::Vector3d& dir)
152 {
153  const double norm = dir.squaredNorm();
154 #if EIGEN_VERSION_AT_LEAST(3, 3, 0)
155  return ((norm - 1) > 1e-9) ? (dir / Eigen::numext::sqrt(norm)) : dir;
156 #else // used in kinetic
157  return ((norm - 1) > 1e-9) ? (dir / sqrt(norm)) : dir;
158 #endif
159 }
160 } // namespace bodies
161 
163 {
164  setDimensionsDirty(shape);
165  updateInternalData();
166 }
167 
169  Eigen::Vector3d& result) const
170 {
171  BoundingSphere bs;
173  for (unsigned int i = 0; i < max_attempts; ++i)
174  {
175  result = Eigen::Vector3d(rng.uniformReal(bs.center.x() - bs.radius, bs.center.x() + bs.radius),
176  rng.uniformReal(bs.center.y() - bs.radius, bs.center.y() + bs.radius),
177  rng.uniformReal(bs.center.z() - bs.radius, bs.center.z() + bs.radius));
178  if (containsPoint(result))
179  return true;
180  }
181  return false;
182 }
183 
184 bool bodies::Sphere::containsPoint(const Eigen::Vector3d& p, bool /* verbose */) const
185 {
186  return (center_ - p).squaredNorm() <= radius2_;
187 }
188 
189 void bodies::Sphere::useDimensions(const shapes::Shape* shape) // radius
190 {
191  radius_ = static_cast<const shapes::Sphere*>(shape)->radius;
192 }
193 
194 std::vector<double> bodies::Sphere::getDimensions() const
195 {
196  std::vector<double> d(1, radius_);
197  return d;
198 }
199 
201 {
202  const auto tmpRadiusU = radius_ * scale_ + padding_;
203  if (tmpRadiusU < 0)
204  throw std::runtime_error("Sphere radius must be non-negative.");
205  radiusU_ = tmpRadiusU;
206  radius2_ = radiusU_ * radiusU_;
207  center_ = pose_.translation();
208 }
209 
210 std::shared_ptr<bodies::Body> bodies::Sphere::cloneAt(const Eigen::Isometry3d& pose, double padding, double scale) const
211 {
212  auto s = std::make_shared<Sphere>();
213  s->radius_ = radius_;
214  s->padding_ = padding;
215  s->scale_ = scale;
216  s->pose_ = pose;
217  s->updateInternalData();
218  return s;
219 }
220 
222 {
223  return 4.0 * boost::math::constants::pi<double>() * radiusU_ * radiusU_ * radiusU_ / 3.0;
224 }
225 
227 {
228  sphere.center = center_;
229  sphere.radius = radiusU_;
230 }
231 
233 {
234  cylinder.pose = pose_;
235  cylinder.radius = radiusU_;
236  cylinder.length = 2.0 * radiusU_;
237 }
238 
240 {
241  bbox.setEmpty();
242 
243  // it's a sphere, so we do not rotate the bounding box
244  Eigen::Isometry3d transform = Eigen::Isometry3d::Identity();
245  transform.translation() = getPose().translation();
246 
247  bbox.extendWithTransformedBox(transform, Eigen::Vector3d(2 * radiusU_, 2 * radiusU_, 2 * radiusU_));
248 }
249 
251  Eigen::Vector3d& result) const
252 {
253  for (unsigned int i = 0; i < max_attempts; ++i)
254  {
255  const double minX = center_.x() - radiusU_;
256  const double maxX = center_.x() + radiusU_;
257  const double minY = center_.y() - radiusU_;
258  const double maxY = center_.y() + radiusU_;
259  const double minZ = center_.z() - radiusU_;
260  const double maxZ = center_.z() + radiusU_;
261  // we are sampling in a box; the probability of success after 20 attempts is 99.99996% given the ratio of box volume
262  // to sphere volume
263  for (int j = 0; j < 20; ++j)
264  {
265  result = Eigen::Vector3d(rng.uniformReal(minX, maxX), rng.uniformReal(minY, maxY), rng.uniformReal(minZ, maxZ));
266  if (containsPoint(result))
267  return true;
268  }
269  }
270  return false;
271 }
272 
273 bool bodies::Sphere::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
274  EigenSTL::vector_Vector3d* intersections, unsigned int count) const
275 {
276  // this is faster than always calling dir.normalized() in case the vector is already unit
277  const Eigen::Vector3d dirNorm = normalize(dir);
278 
279  if (detail::distanceSQR(center_, origin, dirNorm) > radius2_)
280  return false;
281 
282  bool result = false;
283 
284  Eigen::Vector3d cp = origin - center_;
285  double dpcpv = cp.dot(dirNorm);
286 
287  Eigen::Vector3d w = cp - dpcpv * dirNorm;
288  Eigen::Vector3d Q = center_ + w;
289  double x = radius2_ - w.squaredNorm();
290 
291  if (fabs(x) < detail::ZERO)
292  {
293  w = Q - origin;
294  double dpQv = w.dot(dirNorm);
295  if (dpQv > detail::ZERO)
296  {
297  if (intersections)
298  intersections->push_back(Q);
299  result = true;
300  }
301  }
302  else if (x > 0.0)
303  {
304  x = sqrt(x);
305  w = dirNorm * x;
306  Eigen::Vector3d A = Q - w;
307  Eigen::Vector3d B = Q + w;
308  w = A - origin;
309  double dpAv = w.dot(dirNorm);
310  w = B - origin;
311  double dpBv = w.dot(dirNorm);
312 
313  if (dpAv > detail::ZERO)
314  {
315  result = true;
316  if (intersections)
317  {
318  intersections->push_back(A);
319  if (count == 1)
320  return result;
321  }
322  }
323 
324  if (dpBv > detail::ZERO)
325  {
326  result = true;
327  if (intersections)
328  intersections->push_back(B);
329  }
330  }
331  return result;
332 }
333 
334 bool bodies::Cylinder::containsPoint(const Eigen::Vector3d& p, bool /* verbose */) const
335 {
336  Eigen::Vector3d v = p - center_;
337  double pH = v.dot(normalH_);
338 
339  if (fabs(pH) > length2_)
340  return false;
341 
342  double pB1 = v.dot(normalB1_);
343  double remaining = radius2_ - pB1 * pB1;
344 
345  if (remaining < 0.0)
346  return false;
347  else
348  {
349  double pB2 = v.dot(normalB2_);
350  return pB2 * pB2 <= remaining;
351  }
352 }
353 
354 void bodies::Cylinder::useDimensions(const shapes::Shape* shape) // (length, radius)
355 {
356  length_ = static_cast<const shapes::Cylinder*>(shape)->length;
357  radius_ = static_cast<const shapes::Cylinder*>(shape)->radius;
358 }
359 
360 std::vector<double> bodies::Cylinder::getDimensions() const
361 {
362  std::vector<double> d(2);
363  d[0] = radius_;
364  d[1] = length_;
365  return d;
366 }
367 
369 {
370  const auto tmpRadiusU = radius_ * scale_ + padding_;
371  if (tmpRadiusU < 0)
372  throw std::runtime_error("Cylinder radius must be non-negative.");
373  const auto tmpLength2 = scale_ * length_ / 2.0 + padding_;
374  if (tmpLength2 < 0)
375  throw std::runtime_error("Cylinder length must be non-negative.");
376  radiusU_ = tmpRadiusU;
377  length2_ = tmpLength2;
378  radius2_ = radiusU_ * radiusU_;
379  center_ = pose_.translation();
380  radiusBSqr_ = length2_ * length2_ + radius2_;
381  radiusB_ = sqrt(radiusBSqr_);
382 
383  ASSERT_ISOMETRY(pose_);
384  Eigen::Matrix3d basis = pose_.linear();
385  normalB1_ = basis.col(0);
386  normalB2_ = basis.col(1);
387  normalH_ = basis.col(2);
388 
389  double tmp = -normalH_.dot(center_);
390  d1_ = tmp + length2_;
391  d2_ = tmp - length2_;
392 }
393 
395  Eigen::Vector3d& result) const
396 {
397  // sample a point on the base disc of the cylinder
398  double a = rng.uniformReal(-boost::math::constants::pi<double>(), boost::math::constants::pi<double>());
399  double r = rng.uniformReal(-radiusU_, radiusU_);
400  double x = cos(a) * r;
401  double y = sin(a) * r;
402 
403  // sample e height
404  double z = rng.uniformReal(-length2_, length2_);
405 
406  result = pose_ * Eigen::Vector3d(x, y, z);
407  return true;
408 }
409 
410 std::shared_ptr<bodies::Body> bodies::Cylinder::cloneAt(const Eigen::Isometry3d& pose, double padding,
411  double scale) const
412 {
413  auto c = std::make_shared<Cylinder>();
414  c->length_ = length_;
415  c->radius_ = radius_;
416  c->padding_ = padding;
417  c->scale_ = scale;
418  c->pose_ = pose;
419  c->updateInternalData();
420  return c;
421 }
422 
424 {
425  return 2.0 * boost::math::constants::pi<double>() * radius2_ * length2_;
426 }
427 
429 {
430  sphere.center = center_;
431  sphere.radius = radiusB_;
432 }
433 
435 {
436  cylinder.pose = pose_;
437  cylinder.radius = radiusU_;
438  cylinder.length = 2 * length2_;
439 }
440 
442 {
443  bbox.setEmpty();
444 
445  // method taken from http://www.iquilezles.org/www/articles/diskbbox/diskbbox.htm
446 
447  const auto a = normalH_;
448  const auto e = radiusU_ * (Eigen::Vector3d::Ones() - a.cwiseProduct(a) / a.dot(a)).cwiseSqrt();
449  const auto pa = center_ + length2_ * normalH_;
450  const auto pb = center_ - length2_ * normalH_;
451 
452  bbox.extend(pa - e);
453  bbox.extend(pa + e);
454  bbox.extend(pb - e);
455  bbox.extend(pb + e);
456 }
457 
458 bool bodies::Cylinder::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
459  EigenSTL::vector_Vector3d* intersections, unsigned int count) const
460 {
461  // this is faster than always calling dir.normalized() in case the vector is already unit
462  const Eigen::Vector3d dirNorm = normalize(dir);
463 
464  if (detail::distanceSQR(center_, origin, dirNorm) > radiusBSqr_)
465  return false;
466 
467  std::vector<detail::intersc> ipts;
468 
469  // intersect bases
470  double tmp = normalH_.dot(dirNorm);
471  if (fabs(tmp) > detail::ZERO)
472  {
473  double tmp2 = -normalH_.dot(origin);
474  double t1 = (tmp2 - d1_) / tmp;
475 
476  if (t1 > 0.0)
477  {
478  Eigen::Vector3d p1(origin + dirNorm * t1);
479  Eigen::Vector3d v1(p1 - center_);
480  v1 = v1 - normalH_.dot(v1) * normalH_;
481  if (v1.squaredNorm() < radius2_ + detail::ZERO)
482  {
483  if (intersections == nullptr)
484  return true;
485 
486  detail::intersc ip(p1, t1);
487  ipts.push_back(ip);
488  }
489  }
490 
491  double t2 = (tmp2 - d2_) / tmp;
492  if (t2 > 0.0)
493  {
494  Eigen::Vector3d p2(origin + dirNorm * t2);
495  Eigen::Vector3d v2(p2 - center_);
496  v2 = v2 - normalH_.dot(v2) * normalH_;
497  if (v2.squaredNorm() < radius2_ + detail::ZERO)
498  {
499  if (intersections == nullptr)
500  return true;
501 
502  detail::intersc ip(p2, t2);
503  ipts.push_back(ip);
504  }
505  }
506  }
507 
508  if (ipts.size() < 2)
509  {
510  // intersect with infinite cylinder
511  Eigen::Vector3d VD(normalH_.cross(dirNorm));
512  Eigen::Vector3d ROD(normalH_.cross(origin - center_));
513  double a = VD.squaredNorm();
514  double b = 2.0 * ROD.dot(VD);
515  double c = ROD.squaredNorm() - radius2_;
516  double d = b * b - 4.0 * a * c;
517  if (d >= 0.0 && fabs(a) > detail::ZERO)
518  {
519  d = sqrt(d);
520  double e = -a * 2.0;
521  double t1 = (b + d) / e;
522  double t2 = (b - d) / e;
523 
524  if (t1 > 0.0)
525  {
526  Eigen::Vector3d p1(origin + dirNorm * t1);
527  Eigen::Vector3d v1(center_ - p1);
528 
529  if (fabs(normalH_.dot(v1)) < length2_ + detail::ZERO)
530  {
531  if (intersections == nullptr)
532  return true;
533 
534  detail::intersc ip(p1, t1);
535  ipts.push_back(ip);
536  }
537  }
538 
539  if (t2 > 0.0)
540  {
541  Eigen::Vector3d p2(origin + dirNorm * t2);
542  Eigen::Vector3d v2(center_ - p2);
543 
544  if (fabs(normalH_.dot(v2)) < length2_ + detail::ZERO)
545  {
546  if (intersections == nullptr)
547  return true;
548  detail::intersc ip(p2, t2);
549  ipts.push_back(ip);
550  }
551  }
552  }
553  }
554 
555  if (ipts.empty())
556  return false;
557 
558  // If a ray hits exactly the boundary between a side and a base, it is reported twice.
559  // We want to only return the intersection once, thus we need to filter them.
560  detail::filterIntersections(ipts, intersections, count);
561  return true;
562 }
563 
565  Eigen::Vector3d& result) const
566 {
567  result = pose_ * Eigen::Vector3d(rng.uniformReal(-length2_, length2_), rng.uniformReal(-width2_, width2_),
568  rng.uniformReal(-height2_, height2_));
569  return true;
570 }
571 
572 bool bodies::Box::containsPoint(const Eigen::Vector3d& p, bool /* verbose */) const
573 {
574  const Eigen::Vector3d aligned = (pose_.linear().transpose() * (p - center_)).cwiseAbs();
575  return aligned[0] <= length2_ && aligned[1] <= width2_ && aligned[2] <= height2_;
576 }
577 
578 void bodies::Box::useDimensions(const shapes::Shape* shape) // (x, y, z) = (length, width, height)
579 {
580  const double* size = static_cast<const shapes::Box*>(shape)->size;
581  length_ = size[0];
582  width_ = size[1];
583  height_ = size[2];
584 }
585 
586 std::vector<double> bodies::Box::getDimensions() const
587 {
588  std::vector<double> d(3);
589  d[0] = length_;
590  d[1] = width_;
591  d[2] = height_;
592  return d;
593 }
594 
596 {
597  double s2 = scale_ / 2.0;
598  const auto tmpLength2 = length_ * s2 + padding_;
599  const auto tmpWidth2 = width_ * s2 + padding_;
600  const auto tmpHeight2 = height_ * s2 + padding_;
601 
602  if (tmpLength2 < 0 || tmpWidth2 < 0 || tmpHeight2 < 0)
603  throw std::runtime_error("Box dimensions must be non-negative.");
604 
605  length2_ = tmpLength2;
606  width2_ = tmpWidth2;
607  height2_ = tmpHeight2;
608 
609  center_ = pose_.translation();
610 
611  radius2_ = length2_ * length2_ + width2_ * width2_ + height2_ * height2_;
612  radiusB_ = sqrt(radius2_);
613 
614  ASSERT_ISOMETRY(pose_);
615  Eigen::Matrix3d basis = pose_.linear();
616  normalL_ = basis.col(0);
617  normalW_ = basis.col(1);
618  normalH_ = basis.col(2);
619 
620  // rotation is intentionally not applied, the corners are used in intersectsRay()
621  const Eigen::Vector3d tmp(length2_, width2_, height2_);
622  corner1_ = center_ - tmp;
623  corner2_ = center_ + tmp;
624 }
625 
626 std::shared_ptr<bodies::Body> bodies::Box::cloneAt(const Eigen::Isometry3d& pose, double padding, double scale) const
627 {
628  auto b = std::make_shared<Box>();
629  b->length_ = length_;
630  b->width_ = width_;
631  b->height_ = height_;
632  b->padding_ = padding;
633  b->scale_ = scale;
634  b->pose_ = pose;
635  b->updateInternalData();
636  return b;
637 }
638 
640 {
641  return 8.0 * length2_ * width2_ * height2_;
642 }
643 
645 {
646  sphere.center = center_;
647  sphere.radius = radiusB_;
648 }
649 
651 {
652  double a, b;
653 
654  if (length2_ > width2_ && length2_ > height2_)
655  {
656  cylinder.length = length2_ * 2.0;
657  a = width2_;
658  b = height2_;
659  Eigen::Isometry3d rot(Eigen::AngleAxisd(90.0f * (M_PI / 180.0f), Eigen::Vector3d::UnitY()));
660  cylinder.pose = pose_ * rot;
661  }
662  else if (width2_ > height2_)
663  {
664  cylinder.length = width2_ * 2.0;
665  a = height2_;
666  b = length2_;
667  cylinder.radius = sqrt(height2_ * height2_ + length2_ * length2_);
668  Eigen::Isometry3d rot(Eigen::AngleAxisd(90.0f * (M_PI / 180.0f), Eigen::Vector3d::UnitX()));
669  cylinder.pose = pose_ * rot;
670  }
671  else
672  {
673  cylinder.length = height2_ * 2.0;
674  a = width2_;
675  b = length2_;
676  cylinder.pose = pose_;
677  }
678  cylinder.radius = sqrt(a * a + b * b);
679 }
680 
682 {
683  bbox.setEmpty();
684 
685  bbox.extendWithTransformedBox(getPose(), 2 * Eigen::Vector3d(length2_, width2_, height2_));
686 }
687 
688 bool bodies::Box::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
689  EigenSTL::vector_Vector3d* intersections, unsigned int count) const
690 {
691  // this is faster than always calling dir.normalized() in case the vector is already unit
692  const Eigen::Vector3d dirNorm = normalize(dir);
693 
694  // Brian Smits. Efficient bounding box intersection. Ray tracing news 15(1), 2002
695 
696  // The implemented method only works for axis-aligned boxes. So we treat ours as such, cancel its rotation, and
697  // rotate the origin and dir instead. corner1_ and corner2_ are corners with canceled rotation.
698  const Eigen::Matrix3d invRot = pose_.linear().transpose();
699  const Eigen::Vector3d o(invRot * (origin - center_) + center_);
700  const Eigen::Vector3d d(invRot * dirNorm);
701 
702  Eigen::Vector3d tmpTmin, tmpTmax;
703  tmpTmin = (corner1_ - o).cwiseQuotient(d);
704  tmpTmax = (corner2_ - o).cwiseQuotient(d);
705 
706  // In projection to each axis, if the ray has positive direction, it goes from min corner (corner1_) to max corner
707  // (corner2_). If its direction is negative, the first intersection is at max corner and then at min corner.
708  for (size_t i = 0; i < 3; ++i)
709  {
710  if (d[i] < 0)
711  std::swap(tmpTmin[i], tmpTmax[i]);
712  }
713 
714  // tmin and tmax are such values of t in "p = o + t * d" in which the line intersects the box faces.
715  // The box is viewed projected from all three directions, values of t are computed for each of the projections,
716  // and a final constraint on tmin and tmax is updated by each of these projections. If tmin > tmax, there is no
717  // intersection between the line and the box.
718 
719  double tmin, tmax;
720  // use fmax/fmin to handle NaNs which can sneak in when dividing by d in tmpTmin and tmpTmax
721  tmin = std::fmax(tmpTmin.x(), std::fmax(tmpTmin.y(), tmpTmin.z()));
722  tmax = std::fmin(tmpTmax.x(), std::fmin(tmpTmax.y(), tmpTmax.z()));
723 
724  // tmin > tmax, there is no intersection between the line and the box
725  if (tmax - tmin < -detail::ZERO)
726  return false;
727 
728  // As we're doing intersections with a ray and not a line, cases where tmax is negative mean that the intersection is
729  // with the opposite ray and not the one we are working with.
730  if (tmax < 0)
731  return false;
732 
733  if (intersections)
734  {
735  if (tmax - tmin > detail::ZERO)
736  {
737  // tmax > tmin, we have two distinct intersection points
738  if (tmin > detail::ZERO)
739  {
740  // tmin > 0, both intersections lie on the ray
741  intersections->push_back(tmin * dirNorm + origin);
742  if (count == 0 || count > 1)
743  intersections->push_back(tmax * dirNorm + origin);
744  }
745  else
746  {
747  // tmin <= 0 && tmax >= 0, the first intersection point is on the opposite ray and the second one on the correct
748  // ray - this means origin of the ray lies inside the box and we should only report one intersection.
749  intersections->push_back(tmax * dirNorm + origin);
750  }
751  }
752  else
753  {
754  // tmax == tmin, there is exactly one intersection at a corner or edge
755  intersections->push_back(tmax * dirNorm + origin);
756  }
757  }
758 
759  return true;
760 }
761 
762 bool bodies::ConvexMesh::containsPoint(const Eigen::Vector3d& p, bool /* verbose */) const
763 {
764  if (!mesh_data_)
765  return false;
766  if (bounding_box_.containsPoint(p))
767  {
768  // Transform the point to the "base space" of this mesh
769  Eigen::Vector3d ip(i_pose_ * p);
770  return isPointInsidePlanes(ip);
771  }
772  else
773  return false;
774 }
775 
777 {
778  for (unsigned int i = 0; i < mesh_data_->triangles_.size(); i += 3)
779  {
780  Eigen::Vector3d d1 =
781  mesh_data_->vertices_[mesh_data_->triangles_[i]] - mesh_data_->vertices_[mesh_data_->triangles_[i + 1]];
782  Eigen::Vector3d d2 =
783  mesh_data_->vertices_[mesh_data_->triangles_[i]] - mesh_data_->vertices_[mesh_data_->triangles_[i + 2]];
784  // expected computed normal from triangle vertex order
785  Eigen::Vector3d tri_normal = d1.cross(d2);
786  tri_normal.normalize();
787  // actual plane normal
788  Eigen::Vector3d normal(mesh_data_->planes_[mesh_data_->plane_for_triangle_[i / 3]].x(),
789  mesh_data_->planes_[mesh_data_->plane_for_triangle_[i / 3]].y(),
790  mesh_data_->planes_[mesh_data_->plane_for_triangle_[i / 3]].z());
791  bool same_dir = tri_normal.dot(normal) > 0;
792  if (!same_dir)
793  {
794  std::swap(mesh_data_->triangles_[i], mesh_data_->triangles_[i + 1]);
795  }
796  }
797 }
798 
800 {
801  mesh_data_ = std::make_shared<MeshData>();
802  const shapes::Mesh* mesh = static_cast<const shapes::Mesh*>(shape);
803 
804  double maxX = -std::numeric_limits<double>::infinity(), maxY = -std::numeric_limits<double>::infinity(),
805  maxZ = -std::numeric_limits<double>::infinity();
806  double minX = std::numeric_limits<double>::infinity(), minY = std::numeric_limits<double>::infinity(),
807  minZ = std::numeric_limits<double>::infinity();
808 
809  for (unsigned int i = 0; i < mesh->vertex_count; ++i)
810  {
811  double vx = mesh->vertices[3 * i];
812  double vy = mesh->vertices[3 * i + 1];
813  double vz = mesh->vertices[3 * i + 2];
814 
815  if (maxX < vx)
816  maxX = vx;
817  if (maxY < vy)
818  maxY = vy;
819  if (maxZ < vz)
820  maxZ = vz;
821 
822  if (minX > vx)
823  minX = vx;
824  if (minY > vy)
825  minY = vy;
826  if (minZ > vz)
827  minZ = vz;
828  }
829 
830  if (maxX < minX)
831  maxX = minX = 0.0;
832  if (maxY < minY)
833  maxY = minY = 0.0;
834  if (maxZ < minZ)
835  maxZ = minZ = 0.0;
836 
837  mesh_data_->box_size_ = Eigen::Vector3d(maxX - minX, maxY - minY, maxZ - minZ);
838 
839  mesh_data_->box_offset_ = Eigen::Vector3d((minX + maxX) / 2.0, (minY + maxY) / 2.0, (minZ + maxZ) / 2.0);
840 
841  mesh_data_->planes_.clear();
842  mesh_data_->triangles_.clear();
843  mesh_data_->vertices_.clear();
844  mesh_data_->mesh_radiusB_ = 0.0;
845  mesh_data_->mesh_center_ = Eigen::Vector3d();
846 
847  double xdim = maxX - minX;
848  double ydim = maxY - minY;
849  double zdim = maxZ - minZ;
850 
851  double pose1;
852  double pose2;
853 
854  unsigned int off1;
855  unsigned int off2;
856 
857  /* compute bounding cylinder */
858  double cyl_length;
859  double maxdist = -std::numeric_limits<double>::infinity();
860  if (xdim > ydim && xdim > zdim)
861  {
862  off1 = 1;
863  off2 = 2;
864  pose1 = mesh_data_->box_offset_.y();
865  pose2 = mesh_data_->box_offset_.z();
866  cyl_length = xdim;
867  }
868  else if (ydim > zdim)
869  {
870  off1 = 0;
871  off2 = 2;
872  pose1 = mesh_data_->box_offset_.x();
873  pose2 = mesh_data_->box_offset_.z();
874  cyl_length = ydim;
875  }
876  else
877  {
878  off1 = 0;
879  off2 = 1;
880  pose1 = mesh_data_->box_offset_.x();
881  pose2 = mesh_data_->box_offset_.y();
882  cyl_length = zdim;
883  }
884 
885  /* compute convex hull */
886  coordT* points = (coordT*)calloc(mesh->vertex_count * 3, sizeof(coordT));
887  for (unsigned int i = 0; i < mesh->vertex_count; ++i)
888  {
889  points[3 * i + 0] = (coordT)mesh->vertices[3 * i + 0];
890  points[3 * i + 1] = (coordT)mesh->vertices[3 * i + 1];
891  points[3 * i + 2] = (coordT)mesh->vertices[3 * i + 2];
892 
893  double dista = mesh->vertices[3 * i + off1] - pose1;
894  double distb = mesh->vertices[3 * i + off2] - pose2;
895  double dist = sqrt(((dista * dista) + (distb * distb)));
896  if (dist > maxdist)
897  maxdist = dist;
898  }
899  mesh_data_->bounding_cylinder_.radius = maxdist;
900  mesh_data_->bounding_cylinder_.length = cyl_length;
901 
902  static FILE* null = fopen("/dev/null", "w");
903 
904  char flags[] = "qhull Tv Qt";
905  int exitcode = qh_new_qhull(3, mesh->vertex_count, points, true, flags, null, null);
906 
907  if (exitcode != 0)
908  {
909  CONSOLE_BRIDGE_logWarn("Convex hull creation failed");
910  qh_freeqhull(!qh_ALL);
911  int curlong, totlong;
912  qh_memfreeshort(&curlong, &totlong);
913  return;
914  }
915 
916  int num_facets = qh num_facets;
917 
918  int num_vertices = qh num_vertices;
919  mesh_data_->vertices_.reserve(num_vertices);
920  Eigen::Vector3d sum(0, 0, 0);
921 
922  // necessary for FORALLvertices
923  std::map<unsigned int, unsigned int> qhull_vertex_table;
924  vertexT* vertex;
925  FORALLvertices
926  {
927  Eigen::Vector3d vert(vertex->point[0], vertex->point[1], vertex->point[2]);
928  qhull_vertex_table[vertex->id] = mesh_data_->vertices_.size();
929  sum += vert;
930  mesh_data_->vertices_.push_back(vert);
931  }
932 
933  mesh_data_->mesh_center_ = sum / (double)(num_vertices);
934  for (unsigned int j = 0; j < mesh_data_->vertices_.size(); ++j)
935  {
936  double dist = (mesh_data_->vertices_[j] - mesh_data_->mesh_center_).squaredNorm();
937  if (dist > mesh_data_->mesh_radiusB_)
938  mesh_data_->mesh_radiusB_ = dist;
939  }
940 
941  mesh_data_->mesh_radiusB_ = sqrt(mesh_data_->mesh_radiusB_);
942  mesh_data_->triangles_.reserve(num_facets);
943 
944  // HACK: only needed for ABI compatibility with melodic
945  std::map<size_t, size_t>& triangle_for_plane = detail::getTriangleForPlane(this);
946  triangle_for_plane.clear();
947 
948  // neccessary for qhull macro
949  facetT* facet;
950  FORALLfacets
951  {
952  Eigen::Vector4d planeEquation(facet->normal[0], facet->normal[1], facet->normal[2], facet->offset);
953  if (!mesh_data_->planes_.empty())
954  {
955  // filter equal planes - assuming same ones follow each other
956  if ((planeEquation - mesh_data_->planes_.back()).cwiseAbs().maxCoeff() > 1e-6) // max diff to last
957  mesh_data_->planes_.push_back(planeEquation);
958  }
959  else
960  {
961  mesh_data_->planes_.push_back(planeEquation);
962  }
963 
964  // Needed by FOREACHvertex_i_
965  int vertex_n, vertex_i;
966  FOREACHvertex_i_((*facet).vertices)
967  {
968  mesh_data_->triangles_.push_back(qhull_vertex_table[vertex->id]);
969  }
970 
971  mesh_data_->plane_for_triangle_[(mesh_data_->triangles_.size() - 1) / 3] = mesh_data_->planes_.size() - 1;
972  triangle_for_plane[mesh_data_->planes_.size() - 1] = (mesh_data_->triangles_.size() - 1) / 3;
973  }
974  qh_freeqhull(!qh_ALL);
975  int curlong, totlong;
976  qh_memfreeshort(&curlong, &totlong);
977 }
978 
979 std::vector<double> bodies::ConvexMesh::getDimensions() const
980 {
981  return std::vector<double>();
982 }
983 
985 {
986  // compute the scaled vertices, if needed
987  if (padding_ == 0.0 && scale_ == 1.0)
988  {
989  scaled_vertices_ = &mesh_data_->vertices_;
990  return;
991  }
992 
993  if (!scaled_vertices_storage_)
994  scaled_vertices_storage_.reset(new EigenSTL::vector_Vector3d());
995  scaled_vertices_ = scaled_vertices_storage_.get();
996  scaled_vertices_storage_->resize(mesh_data_->vertices_.size());
997  // project vertices along the vertex - center line to the scaled and padded plane
998  // take the average of all tri's planes around that vertex as the result
999  // is not unique
1000 
1001  // First figure out, which tris are connected to each vertex
1002  std::map<unsigned int, std::vector<unsigned int>> vertex_to_tris;
1003  for (unsigned int i = 0; i < mesh_data_->triangles_.size() / 3; ++i)
1004  {
1005  vertex_to_tris[mesh_data_->triangles_[3 * i + 0]].push_back(i);
1006  vertex_to_tris[mesh_data_->triangles_[3 * i + 1]].push_back(i);
1007  vertex_to_tris[mesh_data_->triangles_[3 * i + 2]].push_back(i);
1008  }
1009 
1010  for (unsigned int i = 0; i < mesh_data_->vertices_.size(); ++i)
1011  {
1012  Eigen::Vector3d v(mesh_data_->vertices_[i] - mesh_data_->mesh_center_);
1013  EigenSTL::vector_Vector3d projected_vertices;
1014  for (unsigned int t : vertex_to_tris[i])
1015  {
1016  const Eigen::Vector4d& plane = mesh_data_->planes_[mesh_data_->plane_for_triangle_[t]];
1017  Eigen::Vector3d plane_normal(plane.x(), plane.y(), plane.z());
1018  double d_scaled_padded =
1019  scale_ * plane.w() - (1 - scale_) * mesh_data_->mesh_center_.dot(plane_normal) - padding_;
1020 
1021  // intersect vert - center with scaled/padded plane equation
1022  double denom = v.dot(plane_normal);
1023  if (fabs(denom) < 1e-3)
1024  continue;
1025  double lambda = (-mesh_data_->mesh_center_.dot(plane_normal) - d_scaled_padded) / denom;
1026  Eigen::Vector3d vert_on_plane = v * lambda + mesh_data_->mesh_center_;
1027  projected_vertices.push_back(vert_on_plane);
1028  }
1029  if (projected_vertices.empty())
1030  {
1031  double l = v.norm();
1032  scaled_vertices_storage_->at(i) =
1033  mesh_data_->mesh_center_ + v * (scale_ + (l > detail::ZERO ? padding_ / l : 0.0));
1034  }
1035  else
1036  {
1037  Eigen::Vector3d sum(0, 0, 0);
1038  for (const Eigen::Vector3d& vertex : projected_vertices)
1039  {
1040  sum += vertex;
1041  }
1042  sum /= projected_vertices.size();
1043  scaled_vertices_storage_->at(i) = sum;
1044  }
1045  }
1046 }
1047 
1049 {
1050  if (!mesh_data_)
1051  return;
1052  Eigen::Isometry3d pose = pose_;
1053  pose.translation() = Eigen::Vector3d(pose_ * mesh_data_->box_offset_);
1054 
1055  shapes::Box box_shape(mesh_data_->box_size_.x(), mesh_data_->box_size_.y(), mesh_data_->box_size_.z());
1056  bounding_box_.setPoseDirty(pose);
1057  // The real effect of padding will most likely be smaller due to the mesh padding algorithm, but in "worst case" it
1058  // can inflate the primitive bounding box by the padding_ value.
1059  bounding_box_.setPaddingDirty(padding_);
1060  bounding_box_.setScaleDirty(scale_);
1061  bounding_box_.setDimensionsDirty(&box_shape);
1062  bounding_box_.updateInternalData();
1063 
1064  i_pose_ = pose_.inverse();
1065  center_ = pose_ * mesh_data_->mesh_center_;
1066  radiusB_ = mesh_data_->mesh_radiusB_ * scale_ + padding_;
1067  radiusBSqr_ = radiusB_ * radiusB_;
1068 
1069  // compute the scaled vertices, if needed
1070  if (padding_ == 0.0 && scale_ == 1.0)
1071  scaled_vertices_ = &mesh_data_->vertices_;
1072  else
1073  {
1074  if (!scaled_vertices_storage_)
1075  scaled_vertices_storage_.reset(new EigenSTL::vector_Vector3d());
1076  scaled_vertices_ = scaled_vertices_storage_.get();
1077  scaled_vertices_storage_->resize(mesh_data_->vertices_.size());
1078  for (unsigned int i = 0; i < mesh_data_->vertices_.size(); ++i)
1079  {
1080  Eigen::Vector3d v(mesh_data_->vertices_[i] - mesh_data_->mesh_center_);
1081  double l = v.norm();
1082  scaled_vertices_storage_->at(i) =
1083  mesh_data_->mesh_center_ + v * (scale_ + (l > detail::ZERO ? padding_ / l : 0.0));
1084  }
1085  }
1086 }
1087 const std::vector<unsigned int>& bodies::ConvexMesh::getTriangles() const
1088 {
1089  static const std::vector<unsigned int> empty;
1090  return mesh_data_ ? mesh_data_->triangles_ : empty;
1091 }
1092 
1094 {
1095  static const EigenSTL::vector_Vector3d empty;
1096  return mesh_data_ ? mesh_data_->vertices_ : empty;
1097 }
1098 
1100 {
1101  return scaled_vertices_ ? *scaled_vertices_ : getVertices();
1102 }
1103 
1105 {
1106  static const EigenSTL::vector_Vector4d empty;
1107  return mesh_data_ ? mesh_data_->planes_ : empty;
1108 }
1109 
1110 std::shared_ptr<bodies::Body> bodies::ConvexMesh::cloneAt(const Eigen::Isometry3d& pose, double padding,
1111  double scale) const
1112 {
1113  auto m = std::make_shared<ConvexMesh>();
1114  m->mesh_data_ = mesh_data_;
1115  m->padding_ = padding;
1116  m->scale_ = scale;
1117  m->pose_ = pose;
1118  m->updateInternalData();
1119  return m;
1120 }
1121 
1123 {
1124  sphere.center = center_;
1125  sphere.radius = radiusB_;
1126 }
1127 
1129 {
1130  // the padding contibution might be smaller in reality, but we want to get it right for the worst case
1131  cylinder.length = mesh_data_ ? mesh_data_->bounding_cylinder_.length * scale_ + 2 * padding_ : 0.0;
1132  cylinder.radius = mesh_data_ ? mesh_data_->bounding_cylinder_.radius * scale_ + padding_ : 0.0;
1133  // need to do rotation correctly to get pose, which bounding box does
1134  BoundingCylinder cyl;
1135  bounding_box_.computeBoundingCylinder(cyl);
1136  cylinder.pose = cyl.pose;
1137 }
1138 
1140 {
1141  bbox.setEmpty();
1142 
1143  bounding_box_.computeBoundingBox(bbox);
1144 }
1145 
1146 bool bodies::ConvexMesh::isPointInsidePlanes(const Eigen::Vector3d& point) const
1147 {
1148  unsigned int numplanes = mesh_data_->planes_.size();
1149  const std::map<size_t, size_t>& triangle_for_plane = detail::getTriangleForPlane(this);
1150  for (unsigned int i = 0; i < numplanes; ++i)
1151  {
1152  const Eigen::Vector4d& plane = mesh_data_->planes_[i];
1153  Eigen::Vector3d plane_vec(plane.x(), plane.y(), plane.z());
1154  // w() needs to be recomputed from a scaled vertex as normally it refers to the unscaled plane
1155  // we also cannot simply subtract padding_ from it, because padding of the points on the plane causes a different
1156  // effect than adding padding along this plane's normal (padding effect is direction-dependent)
1157  const auto scaled_point_on_plane = scaled_vertices_->at(mesh_data_->triangles_[3 * triangle_for_plane.at(i)]);
1158  const double w_scaled_padded = -plane_vec.dot(scaled_point_on_plane);
1159  const double dist = plane_vec.dot(point) + w_scaled_padded - detail::ZERO;
1160  if (dist > 0.0)
1161  return false;
1162  }
1163  return true;
1164 }
1165 
1166 unsigned int bodies::ConvexMesh::countVerticesBehindPlane(const Eigen::Vector4f& planeNormal) const
1167 {
1168  unsigned int numvertices = mesh_data_->vertices_.size();
1169  unsigned int result = 0;
1170  for (unsigned int i = 0; i < numvertices; ++i)
1171  {
1172  Eigen::Vector3d plane_vec(planeNormal.x(), planeNormal.y(), planeNormal.z());
1173  double dist = plane_vec.dot(mesh_data_->vertices_[i]) + planeNormal.w() - 1e-6;
1174  if (dist > 0.0)
1175  result++;
1176  }
1177  return result;
1178 }
1179 
1181 {
1182  double volume = 0.0;
1183  if (mesh_data_)
1184  for (unsigned int i = 0; i < mesh_data_->triangles_.size() / 3; ++i)
1185  {
1186  const Eigen::Vector3d& v1 = mesh_data_->vertices_[mesh_data_->triangles_[3 * i + 0]];
1187  const Eigen::Vector3d& v2 = mesh_data_->vertices_[mesh_data_->triangles_[3 * i + 1]];
1188  const Eigen::Vector3d& v3 = mesh_data_->vertices_[mesh_data_->triangles_[3 * i + 2]];
1189  volume += v1.x() * v2.y() * v3.z() + v2.x() * v3.y() * v1.z() + v3.x() * v1.y() * v2.z() -
1190  v1.x() * v3.y() * v2.z() - v2.x() * v1.y() * v3.z() - v3.x() * v2.y() * v1.z();
1191  }
1192  return fabs(volume) / 6.0;
1193 }
1194 
1195 bool bodies::ConvexMesh::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
1196  EigenSTL::vector_Vector3d* intersections, unsigned int count) const
1197 {
1198  // this is faster than always calling dir.normalized() in case the vector is already unit
1199  const Eigen::Vector3d dirNorm = normalize(dir);
1200 
1201  if (!mesh_data_)
1202  return false;
1203  if (detail::distanceSQR(center_, origin, dirNorm) > radiusBSqr_)
1204  return false;
1205  if (!bounding_box_.intersectsRay(origin, dirNorm))
1206  return false;
1207 
1208  // transform the ray into the coordinate frame of the mesh
1209  Eigen::Vector3d orig(i_pose_ * origin);
1210  Eigen::Vector3d dr(i_pose_.linear() * dirNorm);
1211 
1212  std::vector<detail::intersc> ipts;
1213 
1214  bool result = false;
1215 
1216  // for each triangle
1217  const auto nt = mesh_data_->triangles_.size() / 3;
1218  for (size_t i = 0; i < nt; ++i)
1219  {
1220  Eigen::Vector3d vec(mesh_data_->planes_[mesh_data_->plane_for_triangle_[i]].x(),
1221  mesh_data_->planes_[mesh_data_->plane_for_triangle_[i]].y(),
1222  mesh_data_->planes_[mesh_data_->plane_for_triangle_[i]].z());
1223 
1224  const double tmp = vec.dot(dr);
1225  if (fabs(tmp) > detail::ZERO)
1226  {
1227  // planes_[...].w() corresponds to the unscaled mesh, so we need to compute it ourselves
1228  const double w_scaled_padded = vec.dot(scaled_vertices_->at(mesh_data_->triangles_[3 * i]));
1229  const double t = -(vec.dot(orig) + w_scaled_padded) / tmp;
1230  if (t > 0.0)
1231  {
1232  const auto i3 = 3 * i;
1233  const auto v1 = mesh_data_->triangles_[i3 + 0];
1234  const auto v2 = mesh_data_->triangles_[i3 + 1];
1235  const auto v3 = mesh_data_->triangles_[i3 + 2];
1236 
1237  const Eigen::Vector3d& a = scaled_vertices_->at(v1);
1238  const Eigen::Vector3d& b = scaled_vertices_->at(v2);
1239  const Eigen::Vector3d& c = scaled_vertices_->at(v3);
1240 
1241  Eigen::Vector3d cb(c - b);
1242  Eigen::Vector3d ab(a - b);
1243 
1244  // intersection of the plane defined by the triangle and the ray
1245  Eigen::Vector3d P(orig + dr * t);
1246 
1247  // check if it is inside the triangle
1248  Eigen::Vector3d pb(P - b);
1249  Eigen::Vector3d c1(cb.cross(pb));
1250  Eigen::Vector3d c2(cb.cross(ab));
1251  if (c1.dot(c2) < 0.0)
1252  continue;
1253 
1254  Eigen::Vector3d ca(c - a);
1255  Eigen::Vector3d pa(P - a);
1256  Eigen::Vector3d ba(-ab);
1257 
1258  c1 = ca.cross(pa);
1259  c2 = ca.cross(ba);
1260  if (c1.dot(c2) < 0.0)
1261  continue;
1262 
1263  c1 = ba.cross(pa);
1264  c2 = ba.cross(ca);
1265 
1266  if (c1.dot(c2) < 0.0)
1267  continue;
1268 
1269  result = true;
1270  if (intersections)
1271  {
1272  detail::intersc ip(origin + dirNorm * t, t);
1273  ipts.push_back(ip);
1274  }
1275  else
1276  break;
1277  }
1278  }
1279  }
1280 
1281  if (result && intersections)
1282  {
1283  // If a ray hits exactly the boundary between two triangles, it is reported twice;
1284  // We only want return the intersection once; thus we need to filter them.
1285  detail::filterIntersections(ipts, intersections, count);
1286  }
1287 
1288  return result;
1289 }
1290 
1292 {
1293  // HACK: only needed for ABI compatibility with melodic
1294  {
1295  std::lock_guard<std::mutex> lock(detail::g_triangle_for_plane_mutex);
1296  detail::g_triangle_for_plane_.erase(this);
1297  }
1298 }
1299 
1301 {
1302 }
1303 
1304 bodies::BodyVector::BodyVector(const std::vector<shapes::Shape*>& shapes, const EigenSTL::vector_Isometry3d& poses,
1305  double padding)
1306 {
1307  for (unsigned int i = 0; i < shapes.size(); i++)
1308  addBody(shapes[i], poses[i], padding);
1309 }
1310 
1312 {
1313  clear();
1314 }
1315 
1317 {
1318  for (auto& body : bodies_)
1319  delete body;
1320  bodies_.clear();
1321 }
1322 
1324 {
1325  bodies_.push_back(body);
1326  BoundingSphere sphere;
1327  body->computeBoundingSphere(sphere);
1328 }
1329 
1330 void bodies::BodyVector::addBody(const shapes::Shape* shape, const Eigen::Isometry3d& pose, double padding)
1331 {
1333  body->setPoseDirty(pose);
1334  body->setPaddingDirty(padding);
1335  body->updateInternalData();
1336  addBody(body);
1337 }
1338 
1339 std::size_t bodies::BodyVector::getCount() const
1340 {
1341  return bodies_.size();
1342 }
1343 
1344 void bodies::BodyVector::setPose(unsigned int i, const Eigen::Isometry3d& pose)
1345 {
1346  if (i >= bodies_.size())
1347  {
1348  CONSOLE_BRIDGE_logError("There is no body at index %u", i);
1349  return;
1350  }
1351 
1352  bodies_[i]->setPose(pose);
1353 }
1354 
1355 const bodies::Body* bodies::BodyVector::getBody(unsigned int i) const
1356 {
1357  if (i >= bodies_.size())
1358  {
1359  CONSOLE_BRIDGE_logError("There is no body at index %u", i);
1360  return nullptr;
1361  }
1362  else
1363  return bodies_[i];
1364 }
1365 
1366 bool bodies::BodyVector::containsPoint(const Eigen::Vector3d& p, std::size_t& index, bool verbose) const
1367 {
1368  for (std::size_t i = 0; i < bodies_.size(); ++i)
1369  if (bodies_[i]->containsPoint(p, verbose))
1370  {
1371  index = i;
1372  return true;
1373  }
1374  return false;
1375 }
1376 
1377 bool bodies::BodyVector::containsPoint(const Eigen::Vector3d& p, bool verbose) const
1378 {
1379  std::size_t dummy;
1380  return containsPoint(p, dummy, verbose);
1381 }
1382 
1383 bool bodies::BodyVector::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, std::size_t& index,
1384  EigenSTL::vector_Vector3d* intersections, unsigned int count) const
1385 {
1386  for (std::size_t i = 0; i < bodies_.size(); ++i)
1387  if (bodies_[i]->intersectsRay(origin, dir, intersections, count))
1388  {
1389  index = i;
1390  return true;
1391  }
1392  return false;
1393 }
d
Definition of various shapes. No properties such as position are included. These are simply the descr...
bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const override
Check if a point is inside the body. Surface points are included.
Definition: bodies.cpp:572
bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const override
Check if a point is inside the body. Surface points are included.
Definition: bodies.cpp:184
BodyPtr cloneAt(const Eigen::Isometry3d &pose, double padding, double scale) const override
Get a clone of this body, but one that is located at the pose pose and has possibly different passing...
Definition: bodies.cpp:1110
bool intersectsRay(const Eigen::Vector3d &origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections=nullptr, unsigned int count=0) const override
Check if a ray intersects the body, and find the set of intersections, in order, along the ray...
Definition: bodies.cpp:688
virtual bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result) const
Sample a point that is included in the body using a given random number generator.
Definition: bodies.cpp:168
void updateInternalData() override
This function is called every time a change to the body is made, so that intermediate values stored f...
Definition: bodies.cpp:200
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...
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
void filterIntersections(std::vector< detail::intersc > &ipts, EigenSTL::vector_Vector3d *intersections, const size_t count)
Take intersections points in ipts and add them to intersections, filtering duplicates.
Definition: bodies.cpp:117
bool intersectsRay(const Eigen::Vector3d &origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections=nullptr, unsigned int count=0) const override
Check if a ray intersects the body, and find the set of intersections, in order, along the ray...
Definition: bodies.cpp:1195
void computeBoundingBox(AABB &bbox) const override
Compute the axis-aligned bounding box for the body, in its current pose. Scaling and padding are acco...
Definition: bodies.cpp:1139
void computeBoundingCylinder(BoundingCylinder &cylinder) const override
Compute the bounding cylinder for the body, in its current pose. Scaling and padding are accounted fo...
Definition: bodies.cpp:1128
bool isPointInsidePlanes(const Eigen::Vector3d &point) const
Check if the point is inside all halfspaces this mesh consists of (mesh_data_->planes_).
Definition: bodies.cpp:1146
void updateInternalData() override
This function is called every time a change to the body is made, so that intermediate values stored f...
Definition: bodies.cpp:595
static std::map< size_t, size_t > & getTriangleForPlane(const ConvexMesh *mesh)
Definition: bodies.cpp:140
BodyPtr cloneAt(const Eigen::Isometry3d &pose, double padding, double scale) const override
Get a clone of this body, but one that is located at the pose pose and has possibly different passing...
Definition: bodies.cpp:410
const Body * getBody(unsigned int i) const
Get the ith body in the vector.
Definition: bodies.cpp:1355
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:82
intersc(const Eigen::Vector3d &_pt, const double _tm)
Definition: bodies.cpp:92
void computeBoundingBox(AABB &bbox) const override
Compute the axis-aligned bounding box for the body, in its current pose. Scaling and padding are acco...
Definition: bodies.cpp:681
const EigenSTL::vector_Vector3d & getScaledVertices() const
Definition: bodies.cpp:1099
double computeVolume() const override
Compute the volume of the body. This method includes changes induced by scaling and padding...
Definition: bodies.cpp:423
bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result) const override
Sample a point that is included in the body using a given random number generator.
Definition: bodies.cpp:250
#define M_PI
BodyPtr cloneAt(const Eigen::Isometry3d &pose, double padding, double scale) const override
Get a clone of this body, but one that is located at the pose pose and has possibly different passing...
Definition: bodies.cpp:210
const EigenSTL::vector_Vector4d & getPlanes() const
Get the planes that define the convex shape.
Definition: bodies.cpp:1104
bool intersectsRay(const Eigen::Vector3d &origin, const Eigen::Vector3d &dir, std::size_t &index, EigenSTL::vector_Vector3d *intersections=nullptr, 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:1383
bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result) const override
Sample a point that is included in the body using a given random number generator.
Definition: bodies.cpp:394
BodyPtr cloneAt(const Eigen::Isometry3d &pose, double padding, double scale) const override
Get a clone of this body, but one that is located at the pose pose and has possibly different passing...
Definition: bodies.cpp:626
Eigen::Vector3d center
Definition: bodies.h:62
double y
void useDimensions(const shapes::Shape *shape) override
Depending on the shape, this function copies the relevant data to the body.
Definition: bodies.cpp:354
std::vector< double > getDimensions() const override
Get the radius of the sphere.
Definition: bodies.cpp:194
Definition of a sphere that bounds another object.
Definition: bodies.h:60
unsigned int countVerticesBehindPlane(const Eigen::Vector4f &planeNormal) const
(Used mainly for debugging) Count the number of vertices behind a plane
Definition: bodies.cpp:1166
unsigned int index
A basic definition of a shape. Shapes are considered centered at origin.
Definition: shapes.h:77
Definition of a convex mesh. Convex hull is computed for a given shape::Mesh.
Definition: bodies.h:490
virtual void updateInternalData()=0
This function is called every time a change to the body is made, so that intermediate values stored f...
void computeBoundingBox(AABB &bbox) const override
Compute the axis-aligned bounding box for the body, in its current pose. Scaling and padding are acco...
Definition: bodies.cpp:441
void computeBoundingSphere(BoundingSphere &sphere) const override
Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for...
Definition: bodies.cpp:644
Definition of a cylinder.
Definition: bodies.h:69
bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const
Check if any body in the vector contains the input point.
Definition: bodies.cpp:1377
Body * createBodyFromShape(const shapes::Shape *shape)
Create a body from a given shape.
void setPose(unsigned int i, const Eigen::Isometry3d &pose)
Set the pose of a particular body in the vector of bodies.
Definition: bodies.cpp:1344
#define ASSERT_ISOMETRY(transform)
Assert that the given transform is an isometry.
std::vector< double > getDimensions() const override
Returns an empty vector.
Definition: bodies.cpp:979
void computeBoundingSphere(BoundingSphere &sphere) const override
Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for...
Definition: bodies.cpp:1122
double z
void useDimensions(const shapes::Shape *shape) override
Depending on the shape, this function copies the relevant data to the body.
Definition: bodies.cpp:799
void computeBoundingCylinder(BoundingCylinder &cylinder) const override
Compute the bounding cylinder for the body, in its current pose. Scaling and padding are accounted fo...
Definition: bodies.cpp:434
void clear()
Clear all bodies from the vector.
Definition: bodies.cpp:1316
std::size_t getCount() const
Get the number of bodies in this vector.
Definition: bodies.cpp:1339
double uniformReal(double lower_bound, double upper_bound)
Eigen::Vector3d normalize(const Eigen::Vector3d &dir)
Definition: bodies.cpp:151
void updateInternalData() override
This function is called every time a change to the body is made, so that intermediate values stored f...
Definition: bodies.cpp:368
void computeBoundingSphere(BoundingSphere &sphere) const override
Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for...
Definition: bodies.cpp:226
std::unordered_map< const ConvexMesh *, std::map< size_t, size_t > > g_triangle_for_plane_
Definition: bodies.cpp:138
std::vector< Eigen::Vector4d, Eigen::aligned_allocator< Eigen::Vector4d > > vector_Vector4d
bool intersectsRay(const Eigen::Vector3d &origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections=nullptr, unsigned int count=0) const override
Check if a ray intersects the body, and find the set of intersections, in order, along the ray...
Definition: bodies.cpp:458
Definition of a sphere.
Definition: shapes.h:106
void useDimensions(const shapes::Shape *shape) override
Depending on the shape, this function copies the relevant data to the body.
Definition: bodies.cpp:189
void computeBoundingBox(AABB &bbox) const override
Compute the axis-aligned bounding box for the body, in its current pose. Scaling and padding are acco...
Definition: bodies.cpp:239
void setPaddingDirty(double padd)
If the dimension of the body should be padded, this method sets the pading.
Definition: bodies.h:141
double computeVolume() const override
Compute the volume of the body. This method includes changes induced by scaling and padding...
Definition: bodies.cpp:639
void computeBoundingSphere(BoundingSphere &sphere) const override
Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for...
Definition: bodies.cpp:428
void correctVertexOrderFromPlanes()
Definition: bodies.cpp:776
void computeBoundingCylinder(BoundingCylinder &cylinder) const override
Compute the bounding cylinder for the body, in its current pose. Scaling and padding are accounted fo...
Definition: bodies.cpp:232
void extendWithTransformedBox(const Eigen::Isometry3d &transform, const Eigen::Vector3d &box)
Extend with a box transformed by the given transform.
Definition: aabb.cpp:39
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. Padding is not applied to vertices plainly coordinate-wise, but instead the padding value is added to the length of the direction vector between centroid and each vertex.
Definition: shapes.h:281
Eigen::Isometry3d pose
Definition: bodies.h:71
Definition of a box Aligned with the XYZ axes.
Definition: shapes.h:226
bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const override
Check if a point is inside the body. Surface points are included.
Definition: bodies.cpp:762
void setDimensions(const shapes::Shape *shape)
Set the dimensions of the body (from corresponding shape)
Definition: bodies.cpp:162
void computeBoundingCylinder(BoundingCylinder &cylinder) const override
Compute the bounding cylinder for the body, in its current pose. Scaling and padding are accounted fo...
Definition: bodies.cpp:650
#define CONSOLE_BRIDGE_logError(fmt,...)
void setPoseDirty(const Eigen::Isometry3d &pose)
Set the pose of the body.
Definition: bodies.h:168
static const double ZERO
Definition: bodies.cpp:78
Represents an axis-aligned bounding box.
Definition: aabb.h:45
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result) const override
Sample a point that is included in the body using a given random number generator.
Definition: bodies.cpp:564
std::mutex g_triangle_for_plane_mutex
Lock this mutex every time you work with g_triangle_for_plane_.
Definition: bodies.cpp:139
void addBody(Body *body)
Add a body.
Definition: bodies.cpp:1323
void updateInternalData() override
This function is called every time a change to the body is made, so that intermediate values stored f...
Definition: bodies.cpp:1048
bool intersectsRay(const Eigen::Vector3d &origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections=nullptr, unsigned int count=0) const override
Check if a ray intersects the body, and find the set of intersections, in order, along the ray...
Definition: bodies.cpp:273
~ConvexMesh() override
Definition: bodies.cpp:1291
double computeVolume() const override
Compute the volume of the body. This method includes changes induced by scaling and padding...
Definition: bodies.cpp:1180
double computeVolume() const override
Compute the volume of the body. This method includes changes induced by scaling and padding...
Definition: bodies.cpp:221
double x
r
const std::vector< unsigned int > & getTriangles() const
Definition: bodies.cpp:1087
bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const override
Check if a point is inside the body. Surface points are included.
Definition: bodies.cpp:334
bool operator()(const intersc &a, const intersc &b) const
Definition: bodies.cpp:105
A body is a shape + its pose. Point inclusion, ray intersection can be tested, volumes and bounding s...
Definition: bodies.h:88
std::vector< double > getDimensions() const override
Get the radius & length of the cylinder.
Definition: bodies.cpp:360
std::vector< double > getDimensions() const override
Get the length & width & height (x, y, z) of the box.
Definition: bodies.cpp:586
void computeScaledVerticesFromPlaneProjections()
Project the original vertex to the scaled and padded planes and average.
Definition: bodies.cpp:984
void useDimensions(const shapes::Shape *shape) override
Depending on the shape, this function copies the relevant data to the body.
Definition: bodies.cpp:578
const EigenSTL::vector_Vector3d & getVertices() const
Definition: bodies.cpp:1093
This set of classes allows quickly detecting whether a given point is inside an object or not...
Definition: aabb.h:42
Eigen::Vector3d pt
Definition: bodies.cpp:96


geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Tue May 26 2020 03:13:02