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