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