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