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


geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Sun Aug 22 2021 02:42:25