$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00037 #include "geometric_shapes/bodies.h" 00038 00039 #include <ros/console.h> 00040 #include <LinearMath/btConvexHull.h> 00041 // #include <BulletCollision/CollisionShapes/btTriangleShape.h> 00042 #include <algorithm> 00043 #include <iostream> 00044 #include <cmath> 00045 00046 bodies::Body* bodies::createBodyFromShape(const shapes::Shape *shape) 00047 { 00048 Body *body = NULL; 00049 00050 if (shape) 00051 switch (shape->type) 00052 { 00053 case shapes::BOX: 00054 body = new bodies::Box(shape); 00055 break; 00056 case shapes::SPHERE: 00057 body = new bodies::Sphere(shape); 00058 break; 00059 case shapes::CYLINDER: 00060 body = new bodies::Cylinder(shape); 00061 break; 00062 case shapes::MESH: 00063 body = new bodies::ConvexMesh(shape); 00064 break; 00065 default: 00066 ROS_ERROR("Creating body from shape: Unknown shape type %d", (int)shape->type); 00067 break; 00068 } 00069 00070 return body; 00071 } 00072 00073 void bodies::mergeBoundingSpheres(const std::vector<BoundingSphere> &spheres, BoundingSphere &mergedSphere) 00074 { 00075 if (spheres.empty()) 00076 { 00077 mergedSphere.center.setValue(btScalar(0), btScalar(0), btScalar(0)); 00078 mergedSphere.radius = 0.0; 00079 } 00080 else 00081 { 00082 mergedSphere = spheres[0]; 00083 for (unsigned int i = 1 ; i < spheres.size() ; ++i) 00084 { 00085 if (spheres[i].radius <= 0.0) 00086 continue; 00087 double d = spheres[i].center.distance(mergedSphere.center); 00088 if (d + mergedSphere.radius <= spheres[i].radius) 00089 { 00090 mergedSphere.center = spheres[i].center; 00091 mergedSphere.radius = spheres[i].radius; 00092 } 00093 else 00094 if (d + spheres[i].radius > mergedSphere.radius) 00095 { 00096 btVector3 delta = mergedSphere.center - spheres[i].center; 00097 mergedSphere.radius = (delta.length() + spheres[i].radius + mergedSphere.radius)/2.0; 00098 mergedSphere.center = delta.normalized() * (mergedSphere.radius - spheres[i].radius) + spheres[i].center; 00099 } 00100 } 00101 } 00102 } 00103 00104 namespace bodies 00105 { 00106 static const double ZERO = 1e-9; 00107 00110 static inline double distanceSQR(const btVector3& p, const btVector3& origin, const btVector3& dir) 00111 { 00112 btVector3 a = p - origin; 00113 double d = dir.dot(a); 00114 return a.length2() - d * d; 00115 } 00116 00117 namespace detail 00118 { 00119 00120 // temp structure for intersection points (used for ordering them) 00121 struct intersc 00122 { 00123 intersc(const btVector3 &_pt, const double _tm) : pt(_pt), time(_tm) {} 00124 00125 btVector3 pt; 00126 double time; 00127 }; 00128 00129 // define order on intersection points 00130 struct interscOrder 00131 { 00132 bool operator()(const intersc &a, const intersc &b) const 00133 { 00134 return a.time < b.time; 00135 } 00136 }; 00137 } 00138 } 00139 00140 bool bodies::Sphere::containsPoint(const btVector3 &p, bool verbose) const 00141 { 00142 return (m_center - p).length2() < m_radius2; 00143 } 00144 00145 void bodies::Sphere::useDimensions(const shapes::Shape *shape) // radius 00146 { 00147 m_radius = static_cast<const shapes::Sphere*>(shape)->radius; 00148 } 00149 00150 void bodies::Sphere::updateInternalData(void) 00151 { 00152 m_radiusU = m_radius * m_scale + m_padding; 00153 m_radius2 = m_radiusU * m_radiusU; 00154 m_center = m_pose.getOrigin(); 00155 } 00156 00157 double bodies::Sphere::computeVolume(void) const 00158 { 00159 return 4.0 * M_PI * m_radiusU * m_radiusU * m_radiusU / 3.0; 00160 } 00161 00162 void bodies::Sphere::computeBoundingSphere(BoundingSphere &sphere) const 00163 { 00164 sphere.center = m_center; 00165 sphere.radius = m_radiusU; 00166 } 00167 00168 void bodies::Sphere::computeBoundingCylinder(BoundingCylinder &cylinder) const 00169 { 00170 cylinder.pose = m_pose; 00171 cylinder.radius = m_radiusU; 00172 cylinder.length = m_radiusU; 00173 00174 } 00175 00176 bool bodies::Sphere::intersectsRay(const btVector3& origin, const btVector3& dir, std::vector<btVector3> *intersections, unsigned int count) const 00177 { 00178 if (distanceSQR(m_center, origin, dir) > m_radius2) return false; 00179 00180 bool result = false; 00181 00182 btVector3 cp = origin - m_center; 00183 double dpcpv = cp.dot(dir); 00184 00185 btVector3 w = cp - dpcpv * dir; 00186 btVector3 Q = m_center + w; 00187 double x = m_radius2 - w.length2(); 00188 00189 if (fabs(x) < ZERO) 00190 { 00191 w = Q - origin; 00192 double dpQv = w.dot(dir); 00193 if (dpQv > ZERO) 00194 { 00195 if (intersections) 00196 intersections->push_back(Q); 00197 result = true; 00198 } 00199 } else 00200 if (x > 0.0) 00201 { 00202 x = sqrt(x); 00203 w = dir * x; 00204 btVector3 A = Q - w; 00205 btVector3 B = Q + w; 00206 w = A - origin; 00207 double dpAv = w.dot(dir); 00208 w = B - origin; 00209 double dpBv = w.dot(dir); 00210 00211 if (dpAv > ZERO) 00212 { 00213 result = true; 00214 if (intersections) 00215 { 00216 intersections->push_back(A); 00217 if (count == 1) 00218 return result; 00219 } 00220 } 00221 00222 if (dpBv > ZERO) 00223 { 00224 result = true; 00225 if (intersections) 00226 intersections->push_back(B); 00227 } 00228 } 00229 return result; 00230 } 00231 00232 bool bodies::Cylinder::containsPoint(const btVector3 &p, bool verbose) const 00233 { 00234 btVector3 v = p - m_center; 00235 double pH = v.dot(m_normalH); 00236 00237 if (fabs(pH) > m_length2) 00238 return false; 00239 00240 double pB1 = v.dot(m_normalB1); 00241 double remaining = m_radius2 - pB1 * pB1; 00242 00243 if (remaining < 0.0) 00244 return false; 00245 else 00246 { 00247 double pB2 = v.dot(m_normalB2); 00248 return pB2 * pB2 < remaining; 00249 } 00250 } 00251 00252 void bodies::Cylinder::useDimensions(const shapes::Shape *shape) // (length, radius) 00253 { 00254 m_length = static_cast<const shapes::Cylinder*>(shape)->length; 00255 m_radius = static_cast<const shapes::Cylinder*>(shape)->radius; 00256 } 00257 00258 void bodies::Cylinder::updateInternalData(void) 00259 { 00260 m_radiusU = m_radius * m_scale + m_padding; 00261 m_radius2 = m_radiusU * m_radiusU; 00262 m_length2 = m_scale * m_length / 2.0 + m_padding; 00263 m_center = m_pose.getOrigin(); 00264 m_radiusBSqr = m_length2 * m_length2 + m_radius2; 00265 m_radiusB = sqrt(m_radiusBSqr); 00266 00267 const btMatrix3x3& basis = m_pose.getBasis(); 00268 m_normalB1 = basis.getColumn(0); 00269 m_normalB2 = basis.getColumn(1); 00270 m_normalH = basis.getColumn(2); 00271 00272 double tmp = -m_normalH.dot(m_center); 00273 m_d1 = tmp + m_length2; 00274 m_d2 = tmp - m_length2; 00275 } 00276 00277 double bodies::Cylinder::computeVolume(void) const 00278 { 00279 return 2.0 * M_PI * m_radius2 * m_length2; 00280 } 00281 00282 void bodies::Cylinder::computeBoundingSphere(BoundingSphere &sphere) const 00283 { 00284 sphere.center = m_center; 00285 sphere.radius = m_radiusB; 00286 } 00287 00288 void bodies::Cylinder::computeBoundingCylinder(BoundingCylinder &cylinder) const 00289 { 00290 cylinder.pose = m_pose; 00291 cylinder.radius = m_radiusU; 00292 cylinder.length = m_scale*m_length+m_padding; 00293 } 00294 00295 bool bodies::Cylinder::intersectsRay(const btVector3& origin, const btVector3& dir, std::vector<btVector3> *intersections, unsigned int count) const 00296 { 00297 if (distanceSQR(m_center, origin, dir) > m_radiusBSqr) return false; 00298 00299 std::vector<detail::intersc> ipts; 00300 00301 // intersect bases 00302 double tmp = m_normalH.dot(dir); 00303 if (fabs(tmp) > ZERO) 00304 { 00305 double tmp2 = -m_normalH.dot(origin); 00306 double t1 = (tmp2 - m_d1) / tmp; 00307 00308 if (t1 > 0.0) 00309 { 00310 btVector3 p1(origin + dir * t1); 00311 btVector3 v1(p1 - m_center); 00312 v1 = v1 - m_normalH.dot(v1) * m_normalH; 00313 if (v1.length2() < m_radius2 + ZERO) 00314 { 00315 if (intersections == NULL) 00316 return true; 00317 00318 detail::intersc ip(p1, t1); 00319 ipts.push_back(ip); 00320 } 00321 } 00322 00323 double t2 = (tmp2 - m_d2) / tmp; 00324 if (t2 > 0.0) 00325 { 00326 btVector3 p2(origin + dir * t2); 00327 btVector3 v2(p2 - m_center); 00328 v2 = v2 - m_normalH.dot(v2) * m_normalH; 00329 if (v2.length2() < m_radius2 + ZERO) 00330 { 00331 if (intersections == NULL) 00332 return true; 00333 00334 detail::intersc ip(p2, t2); 00335 ipts.push_back(ip); 00336 } 00337 } 00338 } 00339 00340 if (ipts.size() < 2) 00341 { 00342 // intersect with infinite cylinder 00343 btVector3 VD(m_normalH.cross(dir)); 00344 btVector3 ROD(m_normalH.cross(origin - m_center)); 00345 double a = VD.length2(); 00346 double b = 2.0 * ROD.dot(VD); 00347 double c = ROD.length2() - m_radius2; 00348 double d = b * b - 4.0 * a * c; 00349 if (d > 0.0 && fabs(a) > ZERO) 00350 { 00351 d = sqrt(d); 00352 double e = -a * 2.0; 00353 double t1 = (b + d) / e; 00354 double t2 = (b - d) / e; 00355 00356 if (t1 > 0.0) 00357 { 00358 btVector3 p1(origin + dir * t1); 00359 btVector3 v1(m_center - p1); 00360 00361 if (fabs(m_normalH.dot(v1)) < m_length2 + ZERO) 00362 { 00363 if (intersections == NULL) 00364 return true; 00365 00366 detail::intersc ip(p1, t1); 00367 ipts.push_back(ip); 00368 } 00369 } 00370 00371 if (t2 > 0.0) 00372 { 00373 btVector3 p2(origin + dir * t2); 00374 btVector3 v2(m_center - p2); 00375 00376 if (fabs(m_normalH.dot(v2)) < m_length2 + ZERO) 00377 { 00378 if (intersections == NULL) 00379 return true; 00380 detail::intersc ip(p2, t2); 00381 ipts.push_back(ip); 00382 } 00383 } 00384 } 00385 } 00386 00387 if (ipts.empty()) 00388 return false; 00389 00390 std::sort(ipts.begin(), ipts.end(), detail::interscOrder()); 00391 const unsigned int n = count > 0 ? std::min<unsigned int>(count, ipts.size()) : ipts.size(); 00392 for (unsigned int i = 0 ; i < n ; ++i) 00393 intersections->push_back(ipts[i].pt); 00394 00395 return true; 00396 } 00397 00398 bool bodies::Box::containsPoint(const btVector3 &p, bool verbose) const 00399 { 00400 /* if(verbose) 00401 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);*/ 00402 btVector3 v = p - m_center; 00403 double pL = v.dot(m_normalL); 00404 00405 if (fabs(pL) > m_length2) 00406 return false; 00407 00408 double pW = v.dot(m_normalW); 00409 00410 if (fabs(pW) > m_width2) 00411 return false; 00412 00413 double pH = v.dot(m_normalH); 00414 00415 if (fabs(pH) > m_height2) 00416 return false; 00417 00418 return true; 00419 } 00420 00421 void bodies::Box::useDimensions(const shapes::Shape *shape) // (x, y, z) = (length, width, height) 00422 { 00423 const double *size = static_cast<const shapes::Box*>(shape)->size; 00424 m_length = size[0]; 00425 m_width = size[1]; 00426 m_height = size[2]; 00427 } 00428 00429 void bodies::Box::updateInternalData(void) 00430 { 00431 double s2 = m_scale / 2.0; 00432 m_length2 = m_length * s2 + m_padding; 00433 m_width2 = m_width * s2 + m_padding; 00434 m_height2 = m_height * s2 + m_padding; 00435 00436 m_center = m_pose.getOrigin(); 00437 00438 m_radius2 = m_length2 * m_length2 + m_width2 * m_width2 + m_height2 * m_height2; 00439 m_radiusB = sqrt(m_radius2); 00440 00441 const btMatrix3x3& basis = m_pose.getBasis(); 00442 m_normalL = basis.getColumn(0); 00443 m_normalW = basis.getColumn(1); 00444 m_normalH = basis.getColumn(2); 00445 00446 const btVector3 tmp(m_normalL * m_length2 + m_normalW * m_width2 + m_normalH * m_height2); 00447 m_corner1 = m_center - tmp; 00448 m_corner2 = m_center + tmp; 00449 } 00450 00451 double bodies::Box::computeVolume(void) const 00452 { 00453 return 8.0 * m_length2 * m_width2 * m_height2; 00454 } 00455 00456 void bodies::Box::computeBoundingSphere(BoundingSphere &sphere) const 00457 { 00458 sphere.center = m_center; 00459 sphere.radius = m_radiusB; 00460 } 00461 00462 void bodies::Box::computeBoundingCylinder(BoundingCylinder &cylinder) const 00463 { 00464 double a, b; 00465 00466 if(m_length2 > m_width2 && m_length2 > m_height2) { 00467 cylinder.length = m_length2*2.0; 00468 a = m_width2; 00469 b = m_height2; 00470 btTransform rot(btQuaternion(btVector3(0,1,0),btRadians(90))); 00471 cylinder.pose = m_pose*rot; 00472 } else if(m_width2 > m_height2) { 00473 cylinder.length = m_width2*2.0; 00474 a = m_height2; 00475 b = m_length2; 00476 cylinder.radius = sqrt(m_height2*m_height2+m_length2*m_length2); 00477 btTransform rot(btQuaternion(btVector3(1,0,0),btRadians(90))); 00478 cylinder.pose = m_pose*rot; 00479 } else { 00480 cylinder.length = m_height2*2.0; 00481 a = m_width2; 00482 b = m_length2; 00483 cylinder.pose = m_pose; 00484 } 00485 cylinder.radius = sqrt((a)*(a)+(b)*(b)); 00486 //cylinder.radius = sqrt(2*(max_rad*max_rad)); 00487 } 00488 00489 bool bodies::Box::intersectsRay(const btVector3& origin, const btVector3& dir, std::vector<btVector3> *intersections, unsigned int count) const 00490 { 00491 if (distanceSQR(m_center, origin, dir) > m_radius2) return false; 00492 00493 double t_near = -INFINITY; 00494 double t_far = INFINITY; 00495 00496 for (int i = 0; i < 3; i++) 00497 { 00498 const btVector3 &vN = i == 0 ? m_normalL : (i == 1 ? m_normalW : m_normalH); 00499 double dp = vN.dot(dir); 00500 00501 if (fabs(dp) > ZERO) 00502 { 00503 double t1 = vN.dot(m_corner1 - origin) / dp; 00504 double t2 = vN.dot(m_corner2 - origin) / dp; 00505 00506 if (t1 > t2) 00507 std::swap(t1, t2); 00508 00509 if (t1 > t_near) 00510 t_near = t1; 00511 00512 if (t2 < t_far) 00513 t_far = t2; 00514 00515 if (t_near > t_far) 00516 return false; 00517 00518 if (t_far < 0.0) 00519 return false; 00520 } 00521 else 00522 { 00523 if (i == 0) 00524 { 00525 if ((std::min(m_corner1.y(), m_corner2.y()) > origin.y() || 00526 std::max(m_corner1.y(), m_corner2.y()) < origin.y()) && 00527 (std::min(m_corner1.z(), m_corner2.z()) > origin.z() || 00528 std::max(m_corner1.z(), m_corner2.z()) < origin.z())) 00529 return false; 00530 } 00531 else 00532 { 00533 if (i == 1) 00534 { 00535 if ((std::min(m_corner1.x(), m_corner2.x()) > origin.x() || 00536 std::max(m_corner1.x(), m_corner2.x()) < origin.x()) && 00537 (std::min(m_corner1.z(), m_corner2.z()) > origin.z() || 00538 std::max(m_corner1.z(), m_corner2.z()) < origin.z())) 00539 return false; 00540 } 00541 else 00542 if ((std::min(m_corner1.x(), m_corner2.x()) > origin.x() || 00543 std::max(m_corner1.x(), m_corner2.x()) < origin.x()) && 00544 (std::min(m_corner1.y(), m_corner2.y()) > origin.y() || 00545 std::max(m_corner1.y(), m_corner2.y()) < origin.y())) 00546 return false; 00547 } 00548 } 00549 } 00550 00551 if (intersections) 00552 { 00553 if (t_far - t_near > ZERO) 00554 { 00555 intersections->push_back(t_near * dir + origin); 00556 if (count > 1) 00557 intersections->push_back(t_far * dir + origin); 00558 } 00559 else 00560 intersections->push_back(t_far * dir + origin); 00561 } 00562 00563 return true; 00564 } 00565 /* 00566 bool bodies::Mesh::containsPoint(const btVector3 &p) const 00567 { 00568 // compute all intersections 00569 std::vector<btVector3> pts; 00570 intersectsRay(p, btVector3(0, 0, 1), &pts); 00571 00572 // if we have an odd number of intersections, we are inside 00573 return pts.size() % 2 == 1; 00574 } 00575 00576 namespace bodies 00577 { 00578 namespace detail 00579 { 00580 // callback for bullet 00581 class myTriangleCallback : public btTriangleCallback 00582 { 00583 public: 00584 00585 myTriangleCallback(bool keep_triangles) : keep_triangles_(keep_triangles) 00586 { 00587 found_intersections = false; 00588 } 00589 00590 virtual void processTriangle(btVector3 *triangle, int partId, int triangleIndex) 00591 { 00592 found_intersections = true; 00593 if (keep_triangles_) 00594 triangles.push_back(btTriangleShape(triangle[0], triangle[1], triangle[2])); 00596 } 00597 00598 bool found_intersections; 00599 std::vector<btTriangleShape> triangles; 00600 00601 private: 00602 00603 bool keep_triangles_; 00604 }; 00605 00606 } 00607 } 00608 00609 bool bodies::Mesh::intersectsRay(const btVector3& origin, const btVector3& dir, std::vector<btVector3> *intersections, unsigned int count) const 00610 { 00611 if (m_btMeshShape) 00612 { 00613 00614 if (distanceSQR(m_center, origin, dir) > m_radiusBSqr) return false; 00615 00616 // transform the ray into the coordinate frame of the mesh 00617 btVector3 orig(m_iPose * origin); 00618 btVector3 dr(m_iPose.getBasis() * dir); 00619 00620 // find intersection with AABB 00621 btScalar maxT = 0.0; 00622 00623 if (fabs(dr.x()) > ZERO) 00624 { 00625 btScalar t = (m_aabbMax.x() - orig.x()) / dr.x(); 00626 if (t < 0.0) 00627 t = (m_aabbMin.x() - orig.x()) / dr.x(); 00628 if (t > maxT) 00629 maxT = t; 00630 } 00631 if (fabs(dr.y()) > ZERO) 00632 { 00633 btScalar t = (m_aabbMax.y() - orig.y()) / dr.y(); 00634 if (t < 0.0) 00635 t = (m_aabbMin.y() - orig.y()) / dr.y(); 00636 if (t > maxT) 00637 maxT = t; 00638 } 00639 if (fabs(dr.z()) > ZERO) 00640 { 00641 btScalar t = (m_aabbMax.z() - orig.z()) / dr.z(); 00642 if (t < 0.0) 00643 t = (m_aabbMin.z() - orig.z()) / dr.z(); 00644 if (t > maxT) 00645 maxT = t; 00646 } 00647 00648 // the farthest point where we could have an intersection id orig + dr * maxT 00649 00650 detail::myTriangleCallback cb(intersections != NULL); 00651 m_btMeshShape->performRaycast(&cb, orig, orig + dr * maxT); 00652 if (cb.found_intersections) 00653 { 00654 if (intersections) 00655 { 00656 std::vector<detail::intersc> intpt; 00657 for (unsigned int i = 0 ; i < cb.triangles.size() ; ++i) 00658 { 00659 btVector3 normal; 00660 cb.triangles[i].calcNormal(normal); 00661 btScalar dv = normal.dot(dr); 00662 if (fabs(dv) > 1e-3) 00663 { 00664 double t = (normal.dot(cb.triangles[i].getVertexPtr(0)) - normal.dot(orig)) / dv; 00665 // here we use the input origin & direction, since the transform is linear 00666 detail::intersc ip(origin + dir * t, t); 00667 intpt.push_back(ip); 00668 } 00669 } 00670 std::sort(intpt.begin(), intpt.end(), detail::interscOrder()); 00671 const unsigned int n = count > 0 ? std::min<unsigned int>(count, intpt.size()) : intpt.size(); 00672 for (unsigned int i = 0 ; i < n ; ++i) 00673 intersections->push_back(intpt[i].pt); 00674 } 00675 return true; 00676 } 00677 else 00678 return false; 00679 } 00680 else 00681 return false; 00682 } 00683 00684 void bodies::Mesh::useDimensions(const shapes::Shape *shape) 00685 { 00686 const shapes::Mesh *mesh = static_cast<const shapes::Mesh*>(shape); 00687 if (m_btMeshShape) 00688 delete m_btMeshShape; 00689 if (m_btMesh) 00690 delete m_btMesh; 00691 00692 m_btMesh = new btTriangleMesh(); 00693 const unsigned int nt = mesh->triangleCount / 3; 00694 for (unsigned int i = 0 ; i < nt ; ++i) 00695 { 00696 const unsigned int i3 = i *3; 00697 const unsigned int v1 = 3 * mesh->triangles[i3]; 00698 const unsigned int v2 = 3 * mesh->triangles[i3 + 1]; 00699 const unsigned int v3 = 3 * mesh->triangles[i3 + 2]; 00700 m_btMesh->addTriangle(btVector3(mesh->vertices[v1], mesh->vertices[v1 + 1], mesh->vertices[v1 + 2]), 00701 btVector3(mesh->vertices[v2], mesh->vertices[v2 + 1], mesh->vertices[v2 + 2]), 00702 btVector3(mesh->vertices[v3], mesh->vertices[v3 + 1], mesh->vertices[v3 + 2]), true); 00703 } 00704 00705 m_btMeshShape = new btBvhTriangleMeshShape(m_btMesh, true); 00706 } 00707 00708 void bodies::Mesh::updateInternalData(void) 00709 { 00710 if (m_btMeshShape) 00711 { 00712 m_btMeshShape->setLocalScaling(btVector3(m_scale, m_scale, m_scale)); 00713 m_btMeshShape->setMargin(m_padding); 00714 } 00715 m_iPose = m_pose.inverse(); 00716 00717 btTransform id; 00718 id.setIdentity(); 00719 m_btMeshShape->getAabb(id, m_aabbMin, m_aabbMax); 00720 00721 btVector3 d = (m_aabbMax - m_aabbMin) / 2.0; 00722 00723 m_center = m_pose.getOrigin() + d; 00724 m_radiusBSqr = d.length2(); 00725 m_radiusB = sqrt(m_radiusBSqr); 00726 00728 } 00729 00730 double bodies::Mesh::computeVolume(void) const 00731 { 00732 if (m_btMeshShape) 00733 { 00734 // approximation; volume of AABB at identity transform 00735 btVector3 d = m_aabbMax - m_aabbMin; 00736 return d.x() * d.y() * d.z(); 00737 } 00738 else 00739 return 0.0; 00740 } 00741 00742 void bodies::Mesh::computeBoundingSphere(BoundingSphere &sphere) const 00743 { 00744 sphere.center = m_center; 00745 sphere.radius = m_radiusB; 00746 } 00747 00748 */ 00749 00750 bool bodies::ConvexMesh::containsPoint(const btVector3 &p, bool verbose) const 00751 { 00752 if (m_boundingBox.containsPoint(p)) 00753 { 00754 btVector3 ip(m_iPose * p); 00755 ip = m_meshCenter + (ip - m_meshCenter) * m_scale; 00756 return isPointInsidePlanes(ip); 00757 } 00758 else 00759 return false; 00760 } 00761 00762 void bodies::ConvexMesh::useDimensions(const shapes::Shape *shape) 00763 { 00764 const shapes::Mesh *mesh = static_cast<const shapes::Mesh*>(shape); 00765 00766 double maxX = -INFINITY, maxY = -INFINITY, maxZ = -INFINITY; 00767 double minX = INFINITY, minY = INFINITY, minZ = INFINITY; 00768 00769 for(unsigned int i = 0; i < mesh->vertexCount ; ++i) 00770 { 00771 double vx = mesh->vertices[3 * i ]; 00772 double vy = mesh->vertices[3 * i + 1]; 00773 double vz = mesh->vertices[3 * i + 2]; 00774 00775 if (maxX < vx) maxX = vx; 00776 if (maxY < vy) maxY = vy; 00777 if (maxZ < vz) maxZ = vz; 00778 00779 if (minX > vx) minX = vx; 00780 if (minY > vy) minY = vy; 00781 if (minZ > vz) minZ = vz; 00782 } 00783 00784 if (maxX < minX) maxX = minX = 0.0; 00785 if (maxY < minY) maxY = minY = 0.0; 00786 if (maxZ < minZ) maxZ = minZ = 0.0; 00787 00788 shapes::Box *box_shape = new shapes::Box(maxX - minX, maxY - minY, maxZ - minZ); 00789 m_boundingBox.setDimensions(box_shape); 00790 delete box_shape; 00791 00792 m_boxOffset.setValue((minX + maxX) / 2.0, (minY + maxY) / 2.0, (minZ + maxZ) / 2.0); 00793 00794 m_planes.clear(); 00795 m_triangles.clear(); 00796 m_vertices.clear(); 00797 m_meshRadiusB = 0.0; 00798 m_meshCenter.setValue(btScalar(0), btScalar(0), btScalar(0)); 00799 00800 double xdim = maxX - minX; 00801 double ydim = maxY - minY; 00802 double zdim = maxZ - minZ; 00803 00804 double pose1; 00805 double pose2; 00806 00807 unsigned int off1; 00808 unsigned int off2; 00809 00810 double cyl_length; 00811 double maxdist = -INFINITY; 00812 00813 if(xdim > ydim && xdim > zdim) { 00814 00815 off1 = 1; 00816 off2 = 2; 00817 pose1 = m_boxOffset.y(); 00818 pose2 = m_boxOffset.z(); 00819 cyl_length = xdim; 00820 } else if(ydim > zdim) { 00821 off1 = 0; 00822 off2 = 2; 00823 pose1 = m_boxOffset.x(); 00824 pose2 = m_boxOffset.z(); 00825 cyl_length = ydim; 00826 } else { 00827 off1 = 0; 00828 off2 = 1; 00829 pose1 = m_boxOffset.x(); 00830 pose2 = m_boxOffset.y(); 00831 cyl_length = zdim; 00832 } 00833 00834 00835 btVector3 *vertices = new btVector3[mesh->vertexCount]; 00836 for(unsigned int i = 0; i < mesh->vertexCount ; ++i) 00837 { 00838 vertices[i].setX(mesh->vertices[3 * i ]); 00839 vertices[i].setY(mesh->vertices[3 * i + 1]); 00840 vertices[i].setZ(mesh->vertices[3 * i + 2]); 00841 00842 double dista = mesh->vertices[3 * i + off1]-pose1; 00843 double distb = mesh->vertices[3 * i + off2]-pose2; 00844 double dist = sqrt(((dista*dista)+(distb*distb))); 00845 if(dist > maxdist) { 00846 maxdist = dist; 00847 } 00848 } 00849 m_boundingCylinder.radius = maxdist; 00850 m_boundingCylinder.length = cyl_length; 00851 00852 HullDesc hd(QF_TRIANGLES, mesh->vertexCount, vertices); 00853 HullResult hr; 00854 HullLibrary hl; 00855 if (hl.CreateConvexHull(hd, hr) == QE_OK) 00856 { 00857 // std::cout << "Convex hull has " << hr.m_OutputVertices.size() << " vertices (down from " << mesh->vertexCount << "), " << hr.mNumFaces << " faces" << std::endl; 00858 00859 m_vertices.reserve(hr.m_OutputVertices.size()); 00860 btVector3 sum(0, 0, 0); 00861 00862 for (int j = 0 ; j < hr.m_OutputVertices.size() ; ++j) 00863 { 00864 m_vertices.push_back(hr.m_OutputVertices[j]); 00865 sum = sum + hr.m_OutputVertices[j]; 00866 } 00867 00868 m_meshCenter = sum / (double)(hr.m_OutputVertices.size()); 00869 for (unsigned int j = 0 ; j < m_vertices.size() ; ++j) 00870 { 00871 double dist = m_vertices[j].distance2(m_meshCenter); 00872 if (dist > m_meshRadiusB) 00873 m_meshRadiusB = dist; 00874 } 00875 00876 m_meshRadiusB = sqrt(m_meshRadiusB); 00877 m_triangles.reserve(hr.m_Indices.size()); 00878 for (unsigned int j = 0 ; j < hr.mNumFaces ; ++j) 00879 { 00880 const btVector3 &p1 = hr.m_OutputVertices[hr.m_Indices[j * 3 ]]; 00881 const btVector3 &p2 = hr.m_OutputVertices[hr.m_Indices[j * 3 + 1]]; 00882 const btVector3 &p3 = hr.m_OutputVertices[hr.m_Indices[j * 3 + 2]]; 00883 00884 btVector3 edge1 = (p2 - p1); 00885 btVector3 edge2 = (p3 - p1); 00886 00887 edge1.normalize(); 00888 edge2.normalize(); 00889 00890 btVector3 planeNormal = edge1.cross(edge2); 00891 00892 if (planeNormal.length2() > btScalar(1e-6)) 00893 { 00894 planeNormal.normalize(); 00895 btVector4 planeEquation(planeNormal.getX(), planeNormal.getY(), planeNormal.getZ(), -planeNormal.dot(p1)); 00896 00897 unsigned int behindPlane = countVerticesBehindPlane(planeEquation); 00898 if (behindPlane > 0) 00899 { 00900 btVector4 planeEquation2(-planeEquation.getX(), -planeEquation.getY(), -planeEquation.getZ(), -planeEquation.getW()); 00901 unsigned int behindPlane2 = countVerticesBehindPlane(planeEquation2); 00902 if (behindPlane2 < behindPlane) 00903 { 00904 planeEquation.setValue(planeEquation2.getX(), planeEquation2.getY(), planeEquation2.getZ(), planeEquation2.getW()); 00905 behindPlane = behindPlane2; 00906 } 00907 } 00908 00909 if (behindPlane > 0) 00910 ROS_DEBUG("Approximate plane: %d of %d points are behind the plane", behindPlane, (int)m_vertices.size()); 00911 00912 m_planes.push_back(planeEquation); 00913 00914 m_triangles.push_back(hr.m_Indices[j * 3 + 0]); 00915 m_triangles.push_back(hr.m_Indices[j * 3 + 1]); 00916 m_triangles.push_back(hr.m_Indices[j * 3 + 2]); 00917 } 00918 } 00919 } 00920 else 00921 ROS_ERROR("Unable to compute convex hull."); 00922 00923 hl.ReleaseResult(hr); 00924 delete[] vertices; 00925 00926 } 00927 00928 void bodies::ConvexMesh::updateInternalData(void) 00929 { 00930 btTransform pose = m_pose; 00931 pose.setOrigin(m_pose * m_boxOffset); 00932 m_boundingBox.setPose(pose); 00933 m_boundingBox.setPadding(m_padding); 00934 m_boundingBox.setScale(m_scale); 00935 00936 m_iPose = m_pose.inverse(); 00937 m_center = m_pose * m_meshCenter; 00938 m_radiusB = m_meshRadiusB * m_scale + m_padding; 00939 m_radiusBSqr = m_radiusB * m_radiusB; 00940 00941 m_scaledVertices.resize(m_vertices.size()); 00942 for (unsigned int i = 0 ; i < m_vertices.size() ; ++i) 00943 { 00944 btVector3 v(m_vertices[i] - m_meshCenter); 00945 btScalar l = v.length(); 00946 m_scaledVertices[i] = m_meshCenter + v * (m_scale + (l > ZERO ? m_padding / l : 0.0)); 00947 } 00948 } 00949 00950 void bodies::ConvexMesh::computeBoundingSphere(BoundingSphere &sphere) const 00951 { 00952 sphere.center = m_center; 00953 sphere.radius = m_radiusB; 00954 } 00955 00956 void bodies::ConvexMesh::computeBoundingCylinder(BoundingCylinder &cylinder) const 00957 { 00958 cylinder.length = m_boundingCylinder.length; 00959 cylinder.radius = m_boundingCylinder.radius; 00960 //need to do rotation correctly to get pose, which bounding box does 00961 BoundingCylinder cyl; 00962 m_boundingBox.computeBoundingCylinder(cyl); 00963 cylinder.pose = cyl.pose; 00964 } 00965 00966 bool bodies::ConvexMesh::isPointInsidePlanes(const btVector3& point) const 00967 { 00968 unsigned int numplanes = m_planes.size(); 00969 for (unsigned int i = 0 ; i < numplanes ; ++i) 00970 { 00971 const btVector4& plane = m_planes[i]; 00972 btScalar dist = plane.dot(point) + plane.getW() - m_padding - btScalar(1e-6); 00973 if (dist > btScalar(0)) 00974 return false; 00975 } 00976 return true; 00977 } 00978 00979 unsigned int bodies::ConvexMesh::countVerticesBehindPlane(const btVector4& planeNormal) const 00980 { 00981 unsigned int numvertices = m_vertices.size(); 00982 unsigned int result = 0; 00983 for (unsigned int i = 0 ; i < numvertices ; ++i) 00984 { 00985 btScalar dist = planeNormal.dot(m_vertices[i]) + planeNormal.getW() - btScalar(1e-6); 00986 if (dist > btScalar(0)) 00987 result++; 00988 } 00989 return result; 00990 } 00991 00992 double bodies::ConvexMesh::computeVolume(void) const 00993 { 00994 double volume = 0.0; 00995 for (unsigned int i = 0 ; i < m_triangles.size() / 3 ; ++i) 00996 { 00997 const btVector3 &v1 = m_vertices[m_triangles[3*i + 0]]; 00998 const btVector3 &v2 = m_vertices[m_triangles[3*i + 1]]; 00999 const btVector3 &v3 = m_vertices[m_triangles[3*i + 2]]; 01000 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(); 01001 } 01002 return fabs(volume)/6.0; 01003 } 01004 01005 bool bodies::ConvexMesh::intersectsRay(const btVector3& origin, const btVector3& dir, std::vector<btVector3> *intersections, unsigned int count) const 01006 { 01007 if (distanceSQR(m_center, origin, dir) > m_radiusBSqr) return false; 01008 if (!m_boundingBox.intersectsRay(origin, dir)) return false; 01009 01010 // transform the ray into the coordinate frame of the mesh 01011 btVector3 orig(m_iPose * origin); 01012 btVector3 dr(m_iPose.getBasis() * dir); 01013 01014 std::vector<detail::intersc> ipts; 01015 01016 bool result = false; 01017 01018 // for each triangle 01019 const unsigned int nt = m_triangles.size() / 3; 01020 for (unsigned int i = 0 ; i < nt ; ++i) 01021 { 01022 btScalar tmp = m_planes[i].dot(dr); 01023 if (fabs(tmp) > ZERO) 01024 { 01025 double t = -(m_planes[i].dot(orig) + m_planes[i].getW()) / tmp; 01026 if (t > 0.0) 01027 { 01028 const int i3 = 3 * i; 01029 const int v1 = m_triangles[i3 + 0]; 01030 const int v2 = m_triangles[i3 + 1]; 01031 const int v3 = m_triangles[i3 + 2]; 01032 01033 const btVector3 &a = m_scaledVertices[v1]; 01034 const btVector3 &b = m_scaledVertices[v2]; 01035 const btVector3 &c = m_scaledVertices[v3]; 01036 01037 btVector3 cb(c - b); 01038 btVector3 ab(a - b); 01039 01040 // intersection of the plane defined by the triangle and the ray 01041 btVector3 P(orig + dr * t); 01042 01043 // check if it is inside the triangle 01044 btVector3 pb(P - b); 01045 btVector3 c1(cb.cross(pb)); 01046 btVector3 c2(cb.cross(ab)); 01047 if (c1.dot(c2) < 0.0) 01048 continue; 01049 01050 btVector3 ca(c - a); 01051 btVector3 pa(P - a); 01052 btVector3 ba(-ab); 01053 01054 c1 = ca.cross(pa); 01055 c2 = ca.cross(ba); 01056 if (c1.dot(c2) < 0.0) 01057 continue; 01058 01059 c1 = ba.cross(pa); 01060 c2 = ba.cross(ca); 01061 01062 if (c1.dot(c2) < 0.0) 01063 continue; 01064 01065 result = true; 01066 if (intersections) 01067 { 01068 detail::intersc ip(origin + dir * t, t); 01069 ipts.push_back(ip); 01070 } 01071 else 01072 break; 01073 } 01074 } 01075 } 01076 01077 if (intersections) 01078 { 01079 std::sort(ipts.begin(), ipts.end(), detail::interscOrder()); 01080 const unsigned int n = count > 0 ? std::min<unsigned int>(count, ipts.size()) : ipts.size(); 01081 for (unsigned int i = 0 ; i < n ; ++i) 01082 intersections->push_back(ipts[i].pt); 01083 } 01084 01085 return result; 01086 } 01087 01088 bodies::BodyVector::BodyVector(){ 01089 } 01090 01091 bodies::BodyVector::BodyVector(const std::vector<shapes::Shape*>& shapes, 01092 const std::vector<btTransform>& poses, 01093 double padding) 01094 { 01095 padding_ = padding; 01096 for(unsigned int i = 0; i < shapes.size(); i++) { 01097 addBody(shapes[i],poses[i], padding_); 01098 } 01099 } 01100 01101 01102 bodies::BodyVector::~BodyVector() { 01103 for(unsigned int i = 0; i < bodies_.size(); i++) { 01104 delete bodies_[i]; 01105 } 01106 for(unsigned int i = 0; i < padded_bodies_.size(); i++) { 01107 delete padded_bodies_[i]; 01108 } 01109 } 01110 01111 void bodies::BodyVector::addBody(const shapes::Shape* shape, const btTransform& pose, double padding) { 01112 bodies::Body* body = bodies::createBodyFromShape(shape); 01113 body->setPose(pose); 01114 bodies_.push_back(body); 01115 BoundingSphere sphere; 01116 body->computeBoundingSphere(sphere); 01117 rsqrs_.push_back(sphere.radius*sphere.radius); 01118 if(padding > 0.0) { 01119 bodies::Body* padded_body = bodies::createBodyFromShape(shape); 01120 padded_body->setPose(pose); 01121 padded_body->setPadding(padding); 01122 padded_bodies_.push_back(padded_body); 01123 BoundingSphere padded_sphere; 01124 padded_body->computeBoundingSphere(padded_sphere); 01125 padded_rsqrs_.push_back(padded_sphere.radius*padded_sphere.radius); 01126 } 01127 } 01128 01129 void bodies::BodyVector::setPose(unsigned int i, const btTransform& pose) 01130 { 01131 if(i >= bodies_.size()) return; 01132 bodies_[i]->setPose(pose); 01133 if(padding_ > 0.0) { 01134 padded_bodies_[i]->setPose(pose); 01135 } 01136 } 01137 01138 const bodies::Body* bodies::BodyVector::getBody(unsigned int i) const 01139 { 01140 if(i >= bodies_.size()) return NULL; 01141 return bodies_[i]; 01142 } 01143 01144 const bodies::Body* bodies::BodyVector::getPaddedBody(unsigned int i) const 01145 { 01146 if(i >= bodies_.size()) return NULL; 01147 if(padding_ > 0.0) { 01148 return padded_bodies_[i]; 01149 } 01150 return bodies_[i]; 01151 } 01152 01153 01154 bodies::BoundingSphere bodies::BodyVector::getBoundingSphere(unsigned int i) const 01155 { 01156 if(i >= bodies_.size()) { 01157 ROS_WARN("Trying to get sphere for body we don't have. Probably segfault"); 01158 } 01159 BoundingSphere sphere; 01160 bodies_[i]->computeBoundingSphere(sphere); 01161 return sphere; 01162 } 01163 01164 bodies::BoundingSphere bodies::BodyVector::getPaddedBoundingSphere(unsigned int i) const 01165 { 01166 if(i >= bodies_.size()) { 01167 ROS_WARN("Trying to get sphere for body we don't have. Probably segfault"); 01168 } 01169 BoundingSphere sphere; 01170 if(padding_ > 0.0) { 01171 padded_bodies_[i]->computeBoundingSphere(sphere); 01172 } else { 01173 bodies_[i]->computeBoundingSphere(sphere); 01174 } 01175 return sphere; 01176 } 01177 01178 double bodies::BodyVector::getBoundingSphereRadiusSquared(unsigned int i) const 01179 { 01180 if(i >= rsqrs_.size()) { 01181 return -1.0; 01182 } 01183 return rsqrs_[i]; 01184 } 01185 01186 double bodies::BodyVector::getPaddedBoundingSphereRadiusSquared(unsigned int i) const 01187 { 01188 if(i >= rsqrs_.size()) { 01189 return -1.0; 01190 } 01191 if(padding_ > 0.0) { 01192 return padded_rsqrs_[i]; 01193 } 01194 return rsqrs_[i]; 01195 }