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 
38 #include <LinearMath/btConvexHull.h>
39 // #include <BulletCollision/CollisionShapes/btTriangleShape.h>
40 #include <algorithm>
41 #include <iostream>
42 #include <cmath>
43 
45 {
46  Body *body = NULL;
47 
48  if (shape)
49  switch (shape->type)
50  {
51  case shapes::BOX:
52  body = new bodies::Box(shape);
53  break;
54  case shapes::SPHERE:
55  body = new bodies::Sphere(shape);
56  break;
57  case shapes::CYLINDER:
58  body = new bodies::Cylinder(shape);
59  break;
60  case shapes::MESH:
61  body = new bodies::ConvexMesh(shape);
62  break;
63  default:
64  std::cerr << "Creating body from shape: Unknown shape type" << shape->type << std::endl;
65  break;
66  }
67 
68  return body;
69 }
70 
71 void bodies::mergeBoundingSpheres(const std::vector<BoundingSphere> &spheres, BoundingSphere &mergedSphere)
72 {
73  if (spheres.empty())
74  {
75  mergedSphere.center.setValue(tfScalar(0), tfScalar(0), tfScalar(0));
76  mergedSphere.radius = 0.0;
77  }
78  else
79  {
80  mergedSphere = spheres[0];
81  for (unsigned int i = 1 ; i < spheres.size() ; ++i)
82  {
83  if (spheres[i].radius <= 0.0)
84  continue;
85  double d = spheres[i].center.distance(mergedSphere.center);
86  if (d + mergedSphere.radius <= spheres[i].radius)
87  {
88  mergedSphere.center = spheres[i].center;
89  mergedSphere.radius = spheres[i].radius;
90  }
91  else
92  if (d + spheres[i].radius > mergedSphere.radius)
93  {
94  tf::Vector3 delta = mergedSphere.center - spheres[i].center;
95  mergedSphere.radius = (delta.length() + spheres[i].radius + mergedSphere.radius)/2.0;
96  mergedSphere.center = delta.normalized() * (mergedSphere.radius - spheres[i].radius) + spheres[i].center;
97  }
98  }
99  }
100 }
101 
102 namespace bodies
103 {
104  static const double ZERO = 1e-9;
105 
108  static inline double distanceSQR(const tf::Vector3& p, const tf::Vector3& origin, const tf::Vector3& dir)
109  {
110  tf::Vector3 a = p - origin;
111  double d = dir.dot(a);
112  return a.length2() - d * d;
113  }
114 
115  namespace detail
116  {
117 
118  // temp structure for intersection points (used for ordering them)
119  struct intersc
120  {
121  intersc(const tf::Vector3 &_pt, const double _tm) : pt(_pt), time(_tm) {}
122 
124  double time;
125  };
126 
127  // define order on intersection points
129  {
130  bool operator()(const intersc &a, const intersc &b) const
131  {
132  return a.time < b.time;
133  }
134  };
135  }
136 }
137 
138 bool bodies::Sphere::containsPoint(const tf::Vector3 &p, bool verbose) const
139 {
140  return (m_center - p).length2() < m_radius2;
141 }
142 
143 void bodies::Sphere::useDimensions(const shapes::Shape *shape) // radius
144 {
145  m_radius = static_cast<const shapes::Sphere*>(shape)->radius;
146 }
147 
149 {
150  m_radiusU = m_radius * m_scale + m_padding;
151  m_radius2 = m_radiusU * m_radiusU;
152  m_center = m_pose.getOrigin();
153 }
154 
156 {
157  return 4.0 * M_PI * m_radiusU * m_radiusU * m_radiusU / 3.0;
158 }
159 
161 {
162  sphere.center = m_center;
163  sphere.radius = m_radiusU;
164 }
165 
166 bool bodies::Sphere::intersectsRay(const tf::Vector3& origin, const tf::Vector3& dir, std::vector<tf::Vector3> *intersections, unsigned int count) const
167 {
168  if (distanceSQR(m_center, origin, dir) > m_radius2) return false;
169 
170  bool result = false;
171 
172  tf::Vector3 cp = origin - m_center;
173  double dpcpv = cp.dot(dir);
174 
175  tf::Vector3 w = cp - dpcpv * dir;
176  tf::Vector3 Q = m_center + w;
177  double x = m_radius2 - w.length2();
178 
179  if (fabs(x) < ZERO)
180  {
181  w = Q - origin;
182  double dpQv = w.dot(dir);
183  if (dpQv > ZERO)
184  {
185  if (intersections)
186  intersections->push_back(Q);
187  result = true;
188  }
189  } else
190  if (x > 0.0)
191  {
192  x = sqrt(x);
193  w = dir * x;
194  tf::Vector3 A = Q - w;
195  tf::Vector3 B = Q + w;
196  w = A - origin;
197  double dpAv = w.dot(dir);
198  w = B - origin;
199  double dpBv = w.dot(dir);
200 
201  if (dpAv > ZERO)
202  {
203  result = true;
204  if (intersections)
205  {
206  intersections->push_back(A);
207  if (count == 1)
208  return result;
209  }
210  }
211 
212  if (dpBv > ZERO)
213  {
214  result = true;
215  if (intersections)
216  intersections->push_back(B);
217  }
218  }
219  return result;
220 }
221 
222 bool bodies::Cylinder::containsPoint(const tf::Vector3 &p, bool verbose) const
223 {
224  tf::Vector3 v = p - m_center;
225  double pH = v.dot(m_normalH);
226 
227  if (fabs(pH) > m_length2)
228  return false;
229 
230  double pB1 = v.dot(m_normalB1);
231  double remaining = m_radius2 - pB1 * pB1;
232 
233  if (remaining < 0.0)
234  return false;
235  else
236  {
237  double pB2 = v.dot(m_normalB2);
238  return pB2 * pB2 < remaining;
239  }
240 }
241 
242 void bodies::Cylinder::useDimensions(const shapes::Shape *shape) // (length, radius)
243 {
244  m_length = static_cast<const shapes::Cylinder*>(shape)->length;
245  m_radius = static_cast<const shapes::Cylinder*>(shape)->radius;
246 }
247 
249 {
250  m_radiusU = m_radius * m_scale + m_padding;
251  m_radius2 = m_radiusU * m_radiusU;
252  m_length2 = m_scale * m_length / 2.0 + m_padding;
253  m_center = m_pose.getOrigin();
254  m_radiusBSqr = m_length2 * m_length2 + m_radius2;
255  m_radiusB = sqrt(m_radiusBSqr);
256 
257  const tf::Matrix3x3& basis = m_pose.getBasis();
258  m_normalB1 = basis.getColumn(0);
259  m_normalB2 = basis.getColumn(1);
260  m_normalH = basis.getColumn(2);
261 
262  double tmp = -m_normalH.dot(m_center);
263  m_d1 = tmp + m_length2;
264  m_d2 = tmp - m_length2;
265 }
266 
268 {
269  return 2.0 * M_PI * m_radius2 * m_length2;
270 }
271 
273 {
274  sphere.center = m_center;
275  sphere.radius = m_radiusB;
276 }
277 
278 bool bodies::Cylinder::intersectsRay(const tf::Vector3& origin, const tf::Vector3& dir, std::vector<tf::Vector3> *intersections, unsigned int count) const
279 {
280  if (distanceSQR(m_center, origin, dir) > m_radiusBSqr) return false;
281 
282  std::vector<detail::intersc> ipts;
283 
284  // intersect bases
285  double tmp = m_normalH.dot(dir);
286  if (fabs(tmp) > ZERO)
287  {
288  double tmp2 = -m_normalH.dot(origin);
289  double t1 = (tmp2 - m_d1) / tmp;
290 
291  if (t1 > 0.0)
292  {
293  tf::Vector3 p1(origin + dir * t1);
294  tf::Vector3 v1(p1 - m_center);
295  v1 = v1 - m_normalH.dot(v1) * m_normalH;
296  if (v1.length2() < m_radius2 + ZERO)
297  {
298  if (intersections == NULL)
299  return true;
300 
301  detail::intersc ip(p1, t1);
302  ipts.push_back(ip);
303  }
304  }
305 
306  double t2 = (tmp2 - m_d2) / tmp;
307  if (t2 > 0.0)
308  {
309  tf::Vector3 p2(origin + dir * t2);
310  tf::Vector3 v2(p2 - m_center);
311  v2 = v2 - m_normalH.dot(v2) * m_normalH;
312  if (v2.length2() < m_radius2 + ZERO)
313  {
314  if (intersections == NULL)
315  return true;
316 
317  detail::intersc ip(p2, t2);
318  ipts.push_back(ip);
319  }
320  }
321  }
322 
323  if (ipts.size() < 2)
324  {
325  // intersect with infinite cylinder
326  tf::Vector3 VD(m_normalH.cross(dir));
327  tf::Vector3 ROD(m_normalH.cross(origin - m_center));
328  double a = VD.length2();
329  double b = 2.0 * ROD.dot(VD);
330  double c = ROD.length2() - m_radius2;
331  double d = b * b - 4.0 * a * c;
332  if (d > 0.0 && fabs(a) > ZERO)
333  {
334  d = sqrt(d);
335  double e = -a * 2.0;
336  double t1 = (b + d) / e;
337  double t2 = (b - d) / e;
338 
339  if (t1 > 0.0)
340  {
341  tf::Vector3 p1(origin + dir * t1);
342  tf::Vector3 v1(m_center - p1);
343 
344  if (fabs(m_normalH.dot(v1)) < m_length2 + ZERO)
345  {
346  if (intersections == NULL)
347  return true;
348 
349  detail::intersc ip(p1, t1);
350  ipts.push_back(ip);
351  }
352  }
353 
354  if (t2 > 0.0)
355  {
356  tf::Vector3 p2(origin + dir * t2);
357  tf::Vector3 v2(m_center - p2);
358 
359  if (fabs(m_normalH.dot(v2)) < m_length2 + ZERO)
360  {
361  if (intersections == NULL)
362  return true;
363  detail::intersc ip(p2, t2);
364  ipts.push_back(ip);
365  }
366  }
367  }
368  }
369 
370  if (ipts.empty())
371  return false;
372 
373  std::sort(ipts.begin(), ipts.end(), detail::interscOrder());
374  const unsigned int n = count > 0 ? std::min<unsigned int>(count, ipts.size()) : ipts.size();
375  for (unsigned int i = 0 ; i < n ; ++i)
376  intersections->push_back(ipts[i].pt);
377 
378  return true;
379 }
380 
381 bool bodies::Box::containsPoint(const tf::Vector3 &p, bool verbose) const
382 {
383  /* if(verbose)
384  fprintf(stderr,"Actual: %f,%f,%f \nDesired: %f,%f,%f \nTolerance:%f,%f,%f\n",p.x(),p.y(),p.z(),m_center.x(),m_center.y(),m_center.z(),m_length2,m_width2,m_height2);*/
385  tf::Vector3 v = p - m_center;
386  double pL = v.dot(m_normalL);
387 
388  if (fabs(pL) > m_length2)
389  return false;
390 
391  double pW = v.dot(m_normalW);
392 
393  if (fabs(pW) > m_width2)
394  return false;
395 
396  double pH = v.dot(m_normalH);
397 
398  if (fabs(pH) > m_height2)
399  return false;
400 
401  return true;
402 }
403 
404 void bodies::Box::useDimensions(const shapes::Shape *shape) // (x, y, z) = (length, width, height)
405 {
406  const double *size = static_cast<const shapes::Box*>(shape)->size;
407  m_length = size[0];
408  m_width = size[1];
409  m_height = size[2];
410 }
411 
413 {
414  double s2 = m_scale / 2.0;
415  m_length2 = m_length * s2 + m_padding;
416  m_width2 = m_width * s2 + m_padding;
417  m_height2 = m_height * s2 + m_padding;
418 
419  m_center = m_pose.getOrigin();
420 
421  m_radius2 = m_length2 * m_length2 + m_width2 * m_width2 + m_height2 * m_height2;
422  m_radiusB = sqrt(m_radius2);
423 
424  const tf::Matrix3x3& basis = m_pose.getBasis();
425  m_normalL = basis.getColumn(0);
426  m_normalW = basis.getColumn(1);
427  m_normalH = basis.getColumn(2);
428 
429  const tf::Vector3 tmp(m_normalL * m_length2 + m_normalW * m_width2 + m_normalH * m_height2);
430  m_corner1 = m_center - tmp;
431  m_corner2 = m_center + tmp;
432 }
433 
434 double bodies::Box::computeVolume(void) const
435 {
436  return 8.0 * m_length2 * m_width2 * m_height2;
437 }
438 
440 {
441  sphere.center = m_center;
442  sphere.radius = m_radiusB;
443 }
444 
445 bool bodies::Box::intersectsRay(const tf::Vector3& origin, const tf::Vector3& dir, std::vector<tf::Vector3> *intersections, unsigned int count) const
446 {
447  if (distanceSQR(m_center, origin, dir) > m_radius2) return false;
448 
449  double t_near = -INFINITY;
450  double t_far = INFINITY;
451 
452  for (int i = 0; i < 3; i++)
453  {
454  const tf::Vector3 &vN = i == 0 ? m_normalL : (i == 1 ? m_normalW : m_normalH);
455  double dp = vN.dot(dir);
456 
457  if (fabs(dp) > ZERO)
458  {
459  double t1 = vN.dot(m_corner1 - origin) / dp;
460  double t2 = vN.dot(m_corner2 - origin) / dp;
461 
462  if (t1 > t2)
463  std::swap(t1, t2);
464 
465  if (t1 > t_near)
466  t_near = t1;
467 
468  if (t2 < t_far)
469  t_far = t2;
470 
471  if (t_near > t_far)
472  return false;
473 
474  if (t_far < 0.0)
475  return false;
476  }
477  else
478  {
479  if (i == 0)
480  {
481  if ((std::min(m_corner1.y(), m_corner2.y()) > origin.y() ||
482  std::max(m_corner1.y(), m_corner2.y()) < origin.y()) &&
483  (std::min(m_corner1.z(), m_corner2.z()) > origin.z() ||
484  std::max(m_corner1.z(), m_corner2.z()) < origin.z()))
485  return false;
486  }
487  else
488  {
489  if (i == 1)
490  {
491  if ((std::min(m_corner1.x(), m_corner2.x()) > origin.x() ||
492  std::max(m_corner1.x(), m_corner2.x()) < origin.x()) &&
493  (std::min(m_corner1.z(), m_corner2.z()) > origin.z() ||
494  std::max(m_corner1.z(), m_corner2.z()) < origin.z()))
495  return false;
496  }
497  else
498  if ((std::min(m_corner1.x(), m_corner2.x()) > origin.x() ||
499  std::max(m_corner1.x(), m_corner2.x()) < origin.x()) &&
500  (std::min(m_corner1.y(), m_corner2.y()) > origin.y() ||
501  std::max(m_corner1.y(), m_corner2.y()) < origin.y()))
502  return false;
503  }
504  }
505  }
506 
507  if (intersections)
508  {
509  if (t_far - t_near > ZERO)
510  {
511  intersections->push_back(t_near * dir + origin);
512  if (count > 1)
513  intersections->push_back(t_far * dir + origin);
514  }
515  else
516  intersections->push_back(t_far * dir + origin);
517  }
518 
519  return true;
520 }
521 /*
522 bool bodies::Mesh::containsPoint(const tf::Vector3 &p) const
523 {
524  // compute all intersections
525  std::vector<tf::Vector3> pts;
526  intersectsRay(p, tf::Vector3(0, 0, 1), &pts);
527 
528  // if we have an odd number of intersections, we are inside
529  return pts.size() % 2 == 1;
530 }
531 
532 namespace bodies
533 {
534  namespace detail
535  {
536  // callback for bullet
537  class myTriangleCallback : public btTriangleCallback
538  {
539  public:
540 
541  myTriangleCallback(bool keep_triangles) : keep_triangles_(keep_triangles)
542  {
543  found_intersections = false;
544  }
545 
546  virtual void processTriangle(tf::Vector3 *triangle, int partId, int triangleIndex)
547  {
548  found_intersections = true;
549  if (keep_triangles_)
550  triangles.push_back(btTriangleShape(triangle[0], triangle[1], triangle[2]));
552  }
553 
554  bool found_intersections;
555  std::vector<btTriangleShape> triangles;
556 
557  private:
558 
559  bool keep_triangles_;
560  };
561 
562  }
563 }
564 
565 bool bodies::Mesh::intersectsRay(const tf::Vector3& origin, const tf::Vector3& dir, std::vector<tf::Vector3> *intersections, unsigned int count) const
566 {
567  if (m_btMeshShape)
568  {
569 
570  if (distanceSQR(m_center, origin, dir) > m_radiusBSqr) return false;
571 
572  // transform the ray into the coordinate frame of the mesh
573  tf::Vector3 orig(m_iPose * origin);
574  tf::Vector3 dr(m_iPose.getBasis() * dir);
575 
576  // find intersection with AABB
577  tfScalar maxT = 0.0;
578 
579  if (fabs(dr.x()) > ZERO)
580  {
581  tfScalar t = (m_aabbMax.x() - orig.x()) / dr.x();
582  if (t < 0.0)
583  t = (m_aabbMin.x() - orig.x()) / dr.x();
584  if (t > maxT)
585  maxT = t;
586  }
587  if (fabs(dr.y()) > ZERO)
588  {
589  tfScalar t = (m_aabbMax.y() - orig.y()) / dr.y();
590  if (t < 0.0)
591  t = (m_aabbMin.y() - orig.y()) / dr.y();
592  if (t > maxT)
593  maxT = t;
594  }
595  if (fabs(dr.z()) > ZERO)
596  {
597  tfScalar t = (m_aabbMax.z() - orig.z()) / dr.z();
598  if (t < 0.0)
599  t = (m_aabbMin.z() - orig.z()) / dr.z();
600  if (t > maxT)
601  maxT = t;
602  }
603 
604  // the farthest point where we could have an intersection id orig + dr * maxT
605 
606  detail::myTriangleCallback cb(intersections != NULL);
607  m_btMeshShape->performRaycast(&cb, orig, orig + dr * maxT);
608  if (cb.found_intersections)
609  {
610  if (intersections)
611  {
612  std::vector<detail::intersc> intpt;
613  for (unsigned int i = 0 ; i < cb.triangles.size() ; ++i)
614  {
615  tf::Vector3 normal;
616  cb.triangles[i].calcNormal(normal);
617  tfScalar dv = normal.dot(dr);
618  if (fabs(dv) > 1e-3)
619  {
620  double t = (normal.dot(cb.triangles[i].getVertexPtr(0)) - normal.dot(orig)) / dv;
621  // here we use the input origin & direction, since the transform is linear
622  detail::intersc ip(origin + dir * t, t);
623  intpt.push_back(ip);
624  }
625  }
626  std::sort(intpt.begin(), intpt.end(), detail::interscOrder());
627  const unsigned int n = count > 0 ? std::min<unsigned int>(count, intpt.size()) : intpt.size();
628  for (unsigned int i = 0 ; i < n ; ++i)
629  intersections->push_back(intpt[i].pt);
630  }
631  return true;
632  }
633  else
634  return false;
635  }
636  else
637  return false;
638 }
639 
640 void bodies::Mesh::useDimensions(const shapes::Shape *shape)
641 {
642  const shapes::Mesh *mesh = static_cast<const shapes::Mesh*>(shape);
643  if (m_btMeshShape)
644  delete m_btMeshShape;
645  if (m_btMesh)
646  delete m_btMesh;
647 
648  m_btMesh = new btTriangleMesh();
649  const unsigned int nt = mesh->triangleCount / 3;
650  for (unsigned int i = 0 ; i < nt ; ++i)
651  {
652  const unsigned int i3 = i *3;
653  const unsigned int v1 = 3 * mesh->triangles[i3];
654  const unsigned int v2 = 3 * mesh->triangles[i3 + 1];
655  const unsigned int v3 = 3 * mesh->triangles[i3 + 2];
656  m_btMesh->addTriangle(tf::Vector3(mesh->vertices[v1], mesh->vertices[v1 + 1], mesh->vertices[v1 + 2]),
657  tf::Vector3(mesh->vertices[v2], mesh->vertices[v2 + 1], mesh->vertices[v2 + 2]),
658  tf::Vector3(mesh->vertices[v3], mesh->vertices[v3 + 1], mesh->vertices[v3 + 2]), true);
659  }
660 
661  m_btMeshShape = new btBvhTriangleMeshShape(m_btMesh, true);
662 }
663 
664 void bodies::Mesh::updateInternalData(void)
665 {
666  if (m_btMeshShape)
667  {
668  m_btMeshShape->setLocalScaling(tf::Vector3(m_scale, m_scale, m_scale));
669  m_btMeshShape->setMargin(m_padding);
670  }
671  m_iPose = m_pose.inverse();
672 
673  tf::Transform id;
674  id.setIdentity();
675  m_btMeshShape->getAabb(id, m_aabbMin, m_aabbMax);
676 
677  tf::Vector3 d = (m_aabbMax - m_aabbMin) / 2.0;
678 
679  m_center = m_pose.getOrigin() + d;
680  m_radiusBSqr = d.length2();
681  m_radiusB = sqrt(m_radiusBSqr);
682 
684 }
685 
686 double bodies::Mesh::computeVolume(void) const
687 {
688  if (m_btMeshShape)
689  {
690  // approximation; volume of AABB at identity transform
691  tf::Vector3 d = m_aabbMax - m_aabbMin;
692  return d.x() * d.y() * d.z();
693  }
694  else
695  return 0.0;
696 }
697 
698 void bodies::Mesh::computeBoundingSphere(BoundingSphere &sphere) const
699 {
700  sphere.center = m_center;
701  sphere.radius = m_radiusB;
702 }
703 
704 */
705 
706 bool bodies::ConvexMesh::containsPoint(const tf::Vector3 &p, bool verbose) const
707 {
708  if (m_boundingBox.containsPoint(p))
709  {
710  tf::Vector3 ip(m_iPose * p);
711  ip = m_meshCenter + (ip - m_meshCenter) * m_scale;
712  return isPointInsidePlanes(ip);
713  }
714  else
715  return false;
716 }
717 
719 {
720  const shapes::Mesh *mesh = static_cast<const shapes::Mesh*>(shape);
721 
722  double maxX = -INFINITY, maxY = -INFINITY, maxZ = -INFINITY;
723  double minX = INFINITY, minY = INFINITY, minZ = INFINITY;
724 
725  for(unsigned int i = 0; i < mesh->vertexCount ; ++i)
726  {
727  double vx = mesh->vertices[3 * i ];
728  double vy = mesh->vertices[3 * i + 1];
729  double vz = mesh->vertices[3 * i + 2];
730 
731  if (maxX < vx) maxX = vx;
732  if (maxY < vy) maxY = vy;
733  if (maxZ < vz) maxZ = vz;
734 
735  if (minX > vx) minX = vx;
736  if (minY > vy) minY = vy;
737  if (minZ > vz) minZ = vz;
738  }
739 
740  if (maxX < minX) maxX = minX = 0.0;
741  if (maxY < minY) maxY = minY = 0.0;
742  if (maxZ < minZ) maxZ = minZ = 0.0;
743 
744  shapes::Box *box_shape = new shapes::Box(maxX - minX, maxY - minY, maxZ - minZ);
745  m_boundingBox.setDimensions(box_shape);
746  delete box_shape;
747 
748  m_boxOffset.setValue((minX + maxX) / 2.0, (minY + maxY) / 2.0, (minZ + maxZ) / 2.0);
749 
750  m_planes.clear();
751  m_triangles.clear();
752  m_vertices.clear();
753  m_meshRadiusB = 0.0;
754  m_meshCenter.setValue(tfScalar(0), tfScalar(0), tfScalar(0));
755 
756  btVector3 *vertices = new btVector3[mesh->vertexCount];
757  for(unsigned int i = 0; i < mesh->vertexCount ; ++i)
758  {
759  vertices[i].setX(mesh->vertices[3 * i ]);
760  vertices[i].setY(mesh->vertices[3 * i + 1]);
761  vertices[i].setZ(mesh->vertices[3 * i + 2]);
762  }
763 
764  HullDesc hd(QF_TRIANGLES, mesh->vertexCount, vertices);
765  HullResult hr;
766  HullLibrary hl;
767  if (hl.CreateConvexHull(hd, hr) == QE_OK)
768  {
769  // std::cout << "Convex hull has " << hr.m_OutputVertices.size() << " vertices (down from " << mesh->vertexCount << "), " << hr.mNumFaces << " faces" << std::endl;
770 
771  m_vertices.reserve(hr.m_OutputVertices.size());
772  tf::Vector3 sum(0, 0, 0);
773 
774  for (int j = 0 ; j < hr.m_OutputVertices.size() ; ++j)
775  {
776  tf::Vector3 vector3_tmp(tf::Vector3(hr.m_OutputVertices[j][0],hr.m_OutputVertices[j][1],hr.m_OutputVertices[j][2]));
777  m_vertices.push_back(vector3_tmp);
778  sum = sum + vector3_tmp;
779  }
780 
781  m_meshCenter = sum / (double)(hr.m_OutputVertices.size());
782  for (unsigned int j = 0 ; j < m_vertices.size() ; ++j)
783  {
784  double dist = m_vertices[j].distance2(m_meshCenter);
785  if (dist > m_meshRadiusB)
786  m_meshRadiusB = dist;
787  }
788  m_meshRadiusB = sqrt(m_meshRadiusB);
789 
790  m_triangles.reserve(hr.m_Indices.size());
791  for (unsigned int j = 0 ; j < hr.mNumFaces ; ++j)
792  {
793  const tf::Vector3 p1(hr.m_OutputVertices[hr.m_Indices[j * 3 ]][0],hr.m_OutputVertices[hr.m_Indices[j * 3 ]][1],hr.m_OutputVertices[hr.m_Indices[j * 3 ]][2]);
794  const tf::Vector3 p2(hr.m_OutputVertices[hr.m_Indices[j * 3 + 1]][0],hr.m_OutputVertices[hr.m_Indices[j * 3 + 1]][1],hr.m_OutputVertices[hr.m_Indices[j * 3 + 1]][2]);
795  const tf::Vector3 p3(hr.m_OutputVertices[hr.m_Indices[j * 3 + 2]][0],hr.m_OutputVertices[hr.m_Indices[j * 3 + 2]][1],hr.m_OutputVertices[hr.m_Indices[j * 3 + 2]][2]);
796 
797  tf::Vector3 edge1 = (p2 - p1);
798  tf::Vector3 edge2 = (p3 - p1);
799 
800  edge1.normalize();
801  edge2.normalize();
802 
803  tf::Vector3 planeNormal = edge1.cross(edge2);
804 
805  if (planeNormal.length2() > tfScalar(1e-6))
806  {
807  planeNormal.normalize();
808  tf::tfVector4 planeEquation(planeNormal.getX(), planeNormal.getY(), planeNormal.getZ(), -planeNormal.dot(p1));
809 
810  unsigned int behindPlane = countVerticesBehindPlane(planeEquation);
811  if (behindPlane > 0)
812  {
813  tf::tfVector4 planeEquation2(-planeEquation.getX(), -planeEquation.getY(), -planeEquation.getZ(), -planeEquation.getW());
814  unsigned int behindPlane2 = countVerticesBehindPlane(planeEquation2);
815  if (behindPlane2 < behindPlane)
816  {
817  planeEquation.setValue(planeEquation2.getX(), planeEquation2.getY(), planeEquation2.getZ(), planeEquation2.getW());
818  behindPlane = behindPlane2;
819  }
820  }
821 
822  // if (behindPlane > 0)
823  // std::cerr << "Approximate plane: " << behindPlane << " of " << m_vertices.size() << " points are behind the plane" << std::endl;
824 
825  m_planes.push_back(planeEquation);
826 
827  m_triangles.push_back(hr.m_Indices[j * 3 + 0]);
828  m_triangles.push_back(hr.m_Indices[j * 3 + 1]);
829  m_triangles.push_back(hr.m_Indices[j * 3 + 2]);
830  }
831  }
832  }
833  else
834  std::cerr << "Unable to compute convex hull.";
835 
836  hl.ReleaseResult(hr);
837  delete[] vertices;
838 
839 }
840 
842 {
843  tf::Transform pose = m_pose;
844  pose.setOrigin(m_pose * m_boxOffset);
845  m_boundingBox.setPose(pose);
846  m_boundingBox.setPadding(m_padding);
847  m_boundingBox.setScale(m_scale);
848 
849  m_iPose = m_pose.inverse();
850  m_center = m_pose * m_meshCenter;
851  m_radiusB = m_meshRadiusB * m_scale + m_padding;
852  m_radiusBSqr = m_radiusB * m_radiusB;
853 
854  m_scaledVertices.resize(m_vertices.size());
855  for (unsigned int i = 0 ; i < m_vertices.size() ; ++i)
856  {
857  tf::Vector3 v(m_vertices[i] - m_meshCenter);
858  tfScalar l = v.length();
859  m_scaledVertices[i] = m_meshCenter + v * (m_scale + (l > ZERO ? m_padding / l : 0.0));
860  }
861 }
862 
864 {
865  sphere.center = m_center;
866  sphere.radius = m_radiusB;
867 }
868 
870 {
871  unsigned int numplanes = m_planes.size();
872  for (unsigned int i = 0 ; i < numplanes ; ++i)
873  {
874  const tf::tfVector4& plane = m_planes[i];
875  tfScalar dist = plane.dot(point) + plane.getW() - m_padding - tfScalar(1e-6);
876  if (dist > tfScalar(0))
877  return false;
878  }
879  return true;
880 }
881 
882 unsigned int bodies::ConvexMesh::countVerticesBehindPlane(const tf::tfVector4& planeNormal) const
883 {
884  unsigned int numvertices = m_vertices.size();
885  unsigned int result = 0;
886  for (unsigned int i = 0 ; i < numvertices ; ++i)
887  {
888  tfScalar dist = planeNormal.dot(m_vertices[i]) + planeNormal.getW() - tfScalar(1e-6);
889  if (dist > tfScalar(0))
890  result++;
891  }
892  return result;
893 }
894 
896 {
897  double volume = 0.0;
898  for (unsigned int i = 0 ; i < m_triangles.size() / 3 ; ++i)
899  {
900  const tf::Vector3 &v1 = m_vertices[m_triangles[3*i + 0]];
901  const tf::Vector3 &v2 = m_vertices[m_triangles[3*i + 1]];
902  const tf::Vector3 &v3 = m_vertices[m_triangles[3*i + 2]];
903  volume += v1.x()*v2.y()*v3.z() + v2.x()*v3.y()*v1.z() + v3.x()*v1.y()*v2.z() - v1.x()*v3.y()*v2.z() - v2.x()*v1.y()*v3.z() - v3.x()*v2.y()*v1.z();
904  }
905  return fabs(volume)/6.0;
906 }
907 
908 bool bodies::ConvexMesh::intersectsRay(const tf::Vector3& origin, const tf::Vector3& dir, std::vector<tf::Vector3> *intersections, unsigned int count) const
909 {
910  if (distanceSQR(m_center, origin, dir) > m_radiusBSqr) return false;
911  if (!m_boundingBox.intersectsRay(origin, dir)) return false;
912 
913  // transform the ray into the coordinate frame of the mesh
914  tf::Vector3 orig(m_iPose * origin);
915  tf::Vector3 dr(m_iPose.getBasis() * dir);
916 
917  std::vector<detail::intersc> ipts;
918 
919  bool result = false;
920 
921  // for each triangle
922  const unsigned int nt = m_triangles.size() / 3;
923  for (unsigned int i = 0 ; i < nt ; ++i)
924  {
925  tfScalar tmp = m_planes[i].dot(dr);
926  if (fabs(tmp) > ZERO)
927  {
928  double t = -(m_planes[i].dot(orig) + m_planes[i].getW()) / tmp;
929  if (t > 0.0)
930  {
931  const int i3 = 3 * i;
932  const int v1 = m_triangles[i3 + 0];
933  const int v2 = m_triangles[i3 + 1];
934  const int v3 = m_triangles[i3 + 2];
935 
936  const tf::Vector3 &a = m_scaledVertices[v1];
937  const tf::Vector3 &b = m_scaledVertices[v2];
938  const tf::Vector3 &c = m_scaledVertices[v3];
939 
940  tf::Vector3 cb(c - b);
941  tf::Vector3 ab(a - b);
942 
943  // intersection of the plane defined by the triangle and the ray
944  tf::Vector3 P(orig + dr * t);
945 
946  // check if it is inside the triangle
947  tf::Vector3 pb(P - b);
948  tf::Vector3 c1(cb.cross(pb));
949  tf::Vector3 c2(cb.cross(ab));
950  if (c1.dot(c2) < 0.0)
951  continue;
952 
953  tf::Vector3 ca(c - a);
954  tf::Vector3 pa(P - a);
955  tf::Vector3 ba(-ab);
956 
957  c1 = ca.cross(pa);
958  c2 = ca.cross(ba);
959  if (c1.dot(c2) < 0.0)
960  continue;
961 
962  c1 = ba.cross(pa);
963  c2 = ba.cross(ca);
964 
965  if (c1.dot(c2) < 0.0)
966  continue;
967 
968  result = true;
969  if (intersections)
970  {
971  detail::intersc ip(origin + dir * t, t);
972  ipts.push_back(ip);
973  }
974  else
975  break;
976  }
977  }
978  }
979 
980  if (intersections)
981  {
982  std::sort(ipts.begin(), ipts.end(), detail::interscOrder());
983  const unsigned int n = count > 0 ? std::min<unsigned int>(count, ipts.size()) : ipts.size();
984  for (unsigned int i = 0 ; i < n ; ++i)
985  intersections->push_back(ipts[i].pt);
986  }
987 
988  return result;
989 }
d
static double distanceSQR(const tf::Vector3 &p, const tf::Vector3 &origin, const tf::Vector3 &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:108
virtual bool intersectsRay(const tf::Vector3 &origin, const tf::Vector3 &dir, std::vector< tf::Vector3 > *intersections=NULL, unsigned int count=0) const
Check is a ray intersects the body, and find the set of intersections, in order, along the ray...
Definition: bodies.cpp:166
virtual void useDimensions(const shapes::Shape *shape)
Definition: bodies.cpp:143
Definition of a cylinder.
Definition: shapes.h:109
virtual void updateInternalData(void)
Definition: bodies.cpp:841
tf::Vector3 center
Definition: bodies.h:62
double tfScalar
unsigned int countVerticesBehindPlane(const tf::tfVector4 &planeNormal) const
Definition: bodies.cpp:882
Definition of a cylinder.
Definition: bodies.h:209
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:863
virtual void useDimensions(const shapes::Shape *shape)
Definition: bodies.cpp:718
ShapeType type
Definition: shapes.h:70
Definition of a sphere.
Definition: bodies.h:174
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:439
tfScalar getW() const
virtual void updateInternalData(void)
Definition: bodies.cpp:148
TFSIMD_FORCE_INLINE const tfScalar & getY() const
virtual void updateInternalData(void)
Definition: bodies.cpp:412
TFSIMD_FORCE_INLINE const tfScalar & getZ() const
virtual bool intersectsRay(const tf::Vector3 &origin, const tf::Vector3 &dir, std::vector< tf::Vector3 > *intersections=NULL, unsigned int count=0) const
Check is a ray intersects the body, and find the set of intersections, in order, along the ray...
Definition: bodies.cpp:908
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:272
virtual void useDimensions(const shapes::Shape *shape)
Definition: bodies.cpp:404
TFSIMD_FORCE_INLINE void setValue(const tfScalar &x, const tfScalar &y, const tfScalar &z)
virtual bool containsPoint(const tf::Vector3 &p, bool verbose=false) const
Check is a point is inside the body.
Definition: bodies.cpp:706
TFSIMD_FORCE_INLINE Vector3 cross(const Vector3 &v) const
bool operator()(const intersc &a, const intersc &b) const
Definition: bodies.cpp:130
Definition of a sphere that bounds another object.
Definition: bodies.h:60
TFSIMD_FORCE_INLINE const tfScalar & x() const
virtual bool containsPoint(const tf::Vector3 &p, bool verbose=false) const
Check is a point is inside the body.
Definition: bodies.cpp:381
void mergeBoundingSpheres(const std::vector< BoundingSphere > &spheres, BoundingSphere &mergedSphere)
Compute a bounding sphere to enclose a set of bounding spheres.
Definition: bodies.cpp:71
A basic definition of a shape. Shapes are considered centered at origin.
Definition: shapes.h:58
unsigned int vertexCount
the number of available vertices
Definition: shapes.h:185
TFSIMD_FORCE_INLINE tfScalar dot(const Vector3 &v) const
TFSIMD_FORCE_INLINE tfScalar distance2(const Vector3 &v) const
Definition of a convex mesh. Convex hull is computed for a given shape::Mesh.
Definition: bodies.h:354
virtual double computeVolume(void) const
Compute the volume of the body. This method includes changes induced by scaling and padding...
Definition: bodies.cpp:155
TFSIMD_FORCE_INLINE const tfScalar & z() const
bool isPointInsidePlanes(const tf::Vector3 &point) const
Definition: bodies.cpp:869
Definition of a box.
Definition: bodies.h:254
virtual bool intersectsRay(const tf::Vector3 &origin, const tf::Vector3 &dir, std::vector< tf::Vector3 > *intersections=NULL, unsigned int count=0) const
Check is a ray intersects the body, and find the set of intersections, in order, along the ray...
Definition: bodies.cpp:445
virtual bool containsPoint(const tf::Vector3 &p, bool verbose=false) const
Check is a point is inside the body.
Definition: bodies.cpp:222
virtual bool intersectsRay(const tf::Vector3 &origin, const tf::Vector3 &dir, std::vector< tf::Vector3 > *intersections=NULL, unsigned int count=0) const
Check is a ray intersects the body, and find the set of intersections, in order, along the ray...
Definition: bodies.cpp:278
Body * createBodyFromShape(const shapes::Shape *shape)
Create a body from a given shape.
Definition: bodies.cpp:44
TFSIMD_FORCE_INLINE Vector3 normalized() const
virtual double computeVolume(void) const
Compute the volume of the body. This method includes changes induced by scaling and padding...
Definition: bodies.cpp:267
virtual double computeVolume(void) const
Compute the volume of the body. This method includes changes induced by scaling and padding...
Definition: bodies.cpp:895
TFSIMD_FORCE_INLINE const tfScalar & y() const
double * vertices
the position for each vertex vertex k has values at index (3k, 3k+1, 3k+2) = (x,y,z)
Definition: shapes.h:189
TFSIMD_FORCE_INLINE const tfScalar & x() const
static const double ZERO
Definition: bodies.cpp:104
virtual void useDimensions(const shapes::Shape *shape)
Definition: bodies.cpp:242
Definition of a sphere.
Definition: shapes.h:90
TFSIMD_FORCE_INLINE Vector3 & normalize()
TFSIMD_FORCE_INLINE const tfScalar & w() const
Definition of a mesh.
Definition: shapes.h:151
Definition of a box.
Definition: shapes.h:129
intersc(const tf::Vector3 &_pt, const double _tm)
Definition: bodies.cpp:121
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:160
virtual double computeVolume(void) const
Compute the volume of the body. This method includes changes induced by scaling and padding...
Definition: bodies.cpp:434
TFSIMD_FORCE_INLINE const tfScalar & getX() const
virtual void updateInternalData(void)
Definition: bodies.cpp:248
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
virtual bool containsPoint(const tf::Vector3 &p, bool verbose=false) const
Check is a point is inside the body.
Definition: bodies.cpp:138
A body is a shape + its pose. Point inclusion, ray intersection can be tested, volumes and bounding s...
Definition: bodies.h:69
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
TFSIMD_FORCE_INLINE tfScalar length2() const
TFSIMD_FORCE_INLINE tfScalar length() const
Definition: bodies.h:56
TFSIMD_FORCE_INLINE Vector3 getColumn(int i) const


pr2_navigation_self_filter
Author(s): Eitan Marder-Eppstein
autogenerated on Mon Jun 10 2019 14:28:54