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


geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Mon Mar 23 2020 03:16:29