$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 "pr2_navigation_self_filter/bodies.h" 00038 #include <LinearMath/btConvexHull.h> 00039 // #include <BulletCollision/CollisionShapes/btTriangleShape.h> 00040 #include <algorithm> 00041 #include <iostream> 00042 #include <cmath> 00043 00044 bodies::Body* bodies::createBodyFromShape(const shapes::Shape *shape) 00045 { 00046 Body *body = NULL; 00047 00048 if (shape) 00049 switch (shape->type) 00050 { 00051 case shapes::BOX: 00052 body = new bodies::Box(shape); 00053 break; 00054 case shapes::SPHERE: 00055 body = new bodies::Sphere(shape); 00056 break; 00057 case shapes::CYLINDER: 00058 body = new bodies::Cylinder(shape); 00059 break; 00060 case shapes::MESH: 00061 body = new bodies::ConvexMesh(shape); 00062 break; 00063 default: 00064 std::cerr << "Creating body from shape: Unknown shape type" << shape->type << std::endl; 00065 break; 00066 } 00067 00068 return body; 00069 } 00070 00071 void bodies::mergeBoundingSpheres(const std::vector<BoundingSphere> &spheres, BoundingSphere &mergedSphere) 00072 { 00073 if (spheres.empty()) 00074 { 00075 mergedSphere.center.setValue(tfScalar(0), tfScalar(0), tfScalar(0)); 00076 mergedSphere.radius = 0.0; 00077 } 00078 else 00079 { 00080 mergedSphere = spheres[0]; 00081 for (unsigned int i = 1 ; i < spheres.size() ; ++i) 00082 { 00083 if (spheres[i].radius <= 0.0) 00084 continue; 00085 double d = spheres[i].center.distance(mergedSphere.center); 00086 if (d + mergedSphere.radius <= spheres[i].radius) 00087 { 00088 mergedSphere.center = spheres[i].center; 00089 mergedSphere.radius = spheres[i].radius; 00090 } 00091 else 00092 if (d + spheres[i].radius > mergedSphere.radius) 00093 { 00094 tf::Vector3 delta = mergedSphere.center - spheres[i].center; 00095 mergedSphere.radius = (delta.length() + spheres[i].radius + mergedSphere.radius)/2.0; 00096 mergedSphere.center = delta.normalized() * (mergedSphere.radius - spheres[i].radius) + spheres[i].center; 00097 } 00098 } 00099 } 00100 } 00101 00102 namespace bodies 00103 { 00104 static const double ZERO = 1e-9; 00105 00108 static inline double distanceSQR(const tf::Vector3& p, const tf::Vector3& origin, const tf::Vector3& dir) 00109 { 00110 tf::Vector3 a = p - origin; 00111 double d = dir.dot(a); 00112 return a.length2() - d * d; 00113 } 00114 00115 namespace detail 00116 { 00117 00118 // temp structure for intersection points (used for ordering them) 00119 struct intersc 00120 { 00121 intersc(const tf::Vector3 &_pt, const double _tm) : pt(_pt), time(_tm) {} 00122 00123 tf::Vector3 pt; 00124 double time; 00125 }; 00126 00127 // define order on intersection points 00128 struct interscOrder 00129 { 00130 bool operator()(const intersc &a, const intersc &b) const 00131 { 00132 return a.time < b.time; 00133 } 00134 }; 00135 } 00136 } 00137 00138 bool bodies::Sphere::containsPoint(const tf::Vector3 &p, bool verbose) const 00139 { 00140 return (m_center - p).length2() < m_radius2; 00141 } 00142 00143 void bodies::Sphere::useDimensions(const shapes::Shape *shape) // radius 00144 { 00145 m_radius = static_cast<const shapes::Sphere*>(shape)->radius; 00146 } 00147 00148 void bodies::Sphere::updateInternalData(void) 00149 { 00150 m_radiusU = m_radius * m_scale + m_padding; 00151 m_radius2 = m_radiusU * m_radiusU; 00152 m_center = m_pose.getOrigin(); 00153 } 00154 00155 double bodies::Sphere::computeVolume(void) const 00156 { 00157 return 4.0 * M_PI * m_radiusU * m_radiusU * m_radiusU / 3.0; 00158 } 00159 00160 void bodies::Sphere::computeBoundingSphere(BoundingSphere &sphere) const 00161 { 00162 sphere.center = m_center; 00163 sphere.radius = m_radiusU; 00164 } 00165 00166 bool bodies::Sphere::intersectsRay(const tf::Vector3& origin, const tf::Vector3& dir, std::vector<tf::Vector3> *intersections, unsigned int count) const 00167 { 00168 if (distanceSQR(m_center, origin, dir) > m_radius2) return false; 00169 00170 bool result = false; 00171 00172 tf::Vector3 cp = origin - m_center; 00173 double dpcpv = cp.dot(dir); 00174 00175 tf::Vector3 w = cp - dpcpv * dir; 00176 tf::Vector3 Q = m_center + w; 00177 double x = m_radius2 - w.length2(); 00178 00179 if (fabs(x) < ZERO) 00180 { 00181 w = Q - origin; 00182 double dpQv = w.dot(dir); 00183 if (dpQv > ZERO) 00184 { 00185 if (intersections) 00186 intersections->push_back(Q); 00187 result = true; 00188 } 00189 } else 00190 if (x > 0.0) 00191 { 00192 x = sqrt(x); 00193 w = dir * x; 00194 tf::Vector3 A = Q - w; 00195 tf::Vector3 B = Q + w; 00196 w = A - origin; 00197 double dpAv = w.dot(dir); 00198 w = B - origin; 00199 double dpBv = w.dot(dir); 00200 00201 if (dpAv > ZERO) 00202 { 00203 result = true; 00204 if (intersections) 00205 { 00206 intersections->push_back(A); 00207 if (count == 1) 00208 return result; 00209 } 00210 } 00211 00212 if (dpBv > ZERO) 00213 { 00214 result = true; 00215 if (intersections) 00216 intersections->push_back(B); 00217 } 00218 } 00219 return result; 00220 } 00221 00222 bool bodies::Cylinder::containsPoint(const tf::Vector3 &p, bool verbose) const 00223 { 00224 tf::Vector3 v = p - m_center; 00225 double pH = v.dot(m_normalH); 00226 00227 if (fabs(pH) > m_length2) 00228 return false; 00229 00230 double pB1 = v.dot(m_normalB1); 00231 double remaining = m_radius2 - pB1 * pB1; 00232 00233 if (remaining < 0.0) 00234 return false; 00235 else 00236 { 00237 double pB2 = v.dot(m_normalB2); 00238 return pB2 * pB2 < remaining; 00239 } 00240 } 00241 00242 void bodies::Cylinder::useDimensions(const shapes::Shape *shape) // (length, radius) 00243 { 00244 m_length = static_cast<const shapes::Cylinder*>(shape)->length; 00245 m_radius = static_cast<const shapes::Cylinder*>(shape)->radius; 00246 } 00247 00248 void bodies::Cylinder::updateInternalData(void) 00249 { 00250 m_radiusU = m_radius * m_scale + m_padding; 00251 m_radius2 = m_radiusU * m_radiusU; 00252 m_length2 = m_scale * m_length / 2.0 + m_padding; 00253 m_center = m_pose.getOrigin(); 00254 m_radiusBSqr = m_length2 * m_length2 + m_radius2; 00255 m_radiusB = sqrt(m_radiusBSqr); 00256 00257 const tf::Matrix3x3& basis = m_pose.getBasis(); 00258 m_normalB1 = basis.getColumn(0); 00259 m_normalB2 = basis.getColumn(1); 00260 m_normalH = basis.getColumn(2); 00261 00262 double tmp = -m_normalH.dot(m_center); 00263 m_d1 = tmp + m_length2; 00264 m_d2 = tmp - m_length2; 00265 } 00266 00267 double bodies::Cylinder::computeVolume(void) const 00268 { 00269 return 2.0 * M_PI * m_radius2 * m_length2; 00270 } 00271 00272 void bodies::Cylinder::computeBoundingSphere(BoundingSphere &sphere) const 00273 { 00274 sphere.center = m_center; 00275 sphere.radius = m_radiusB; 00276 } 00277 00278 bool bodies::Cylinder::intersectsRay(const tf::Vector3& origin, const tf::Vector3& dir, std::vector<tf::Vector3> *intersections, unsigned int count) const 00279 { 00280 if (distanceSQR(m_center, origin, dir) > m_radiusBSqr) return false; 00281 00282 std::vector<detail::intersc> ipts; 00283 00284 // intersect bases 00285 double tmp = m_normalH.dot(dir); 00286 if (fabs(tmp) > ZERO) 00287 { 00288 double tmp2 = -m_normalH.dot(origin); 00289 double t1 = (tmp2 - m_d1) / tmp; 00290 00291 if (t1 > 0.0) 00292 { 00293 tf::Vector3 p1(origin + dir * t1); 00294 tf::Vector3 v1(p1 - m_center); 00295 v1 = v1 - m_normalH.dot(v1) * m_normalH; 00296 if (v1.length2() < m_radius2 + ZERO) 00297 { 00298 if (intersections == NULL) 00299 return true; 00300 00301 detail::intersc ip(p1, t1); 00302 ipts.push_back(ip); 00303 } 00304 } 00305 00306 double t2 = (tmp2 - m_d2) / tmp; 00307 if (t2 > 0.0) 00308 { 00309 tf::Vector3 p2(origin + dir * t2); 00310 tf::Vector3 v2(p2 - m_center); 00311 v2 = v2 - m_normalH.dot(v2) * m_normalH; 00312 if (v2.length2() < m_radius2 + ZERO) 00313 { 00314 if (intersections == NULL) 00315 return true; 00316 00317 detail::intersc ip(p2, t2); 00318 ipts.push_back(ip); 00319 } 00320 } 00321 } 00322 00323 if (ipts.size() < 2) 00324 { 00325 // intersect with infinite cylinder 00326 tf::Vector3 VD(m_normalH.cross(dir)); 00327 tf::Vector3 ROD(m_normalH.cross(origin - m_center)); 00328 double a = VD.length2(); 00329 double b = 2.0 * ROD.dot(VD); 00330 double c = ROD.length2() - m_radius2; 00331 double d = b * b - 4.0 * a * c; 00332 if (d > 0.0 && fabs(a) > ZERO) 00333 { 00334 d = sqrt(d); 00335 double e = -a * 2.0; 00336 double t1 = (b + d) / e; 00337 double t2 = (b - d) / e; 00338 00339 if (t1 > 0.0) 00340 { 00341 tf::Vector3 p1(origin + dir * t1); 00342 tf::Vector3 v1(m_center - p1); 00343 00344 if (fabs(m_normalH.dot(v1)) < m_length2 + ZERO) 00345 { 00346 if (intersections == NULL) 00347 return true; 00348 00349 detail::intersc ip(p1, t1); 00350 ipts.push_back(ip); 00351 } 00352 } 00353 00354 if (t2 > 0.0) 00355 { 00356 tf::Vector3 p2(origin + dir * t2); 00357 tf::Vector3 v2(m_center - p2); 00358 00359 if (fabs(m_normalH.dot(v2)) < m_length2 + ZERO) 00360 { 00361 if (intersections == NULL) 00362 return true; 00363 detail::intersc ip(p2, t2); 00364 ipts.push_back(ip); 00365 } 00366 } 00367 } 00368 } 00369 00370 if (ipts.empty()) 00371 return false; 00372 00373 std::sort(ipts.begin(), ipts.end(), detail::interscOrder()); 00374 const unsigned int n = count > 0 ? std::min<unsigned int>(count, ipts.size()) : ipts.size(); 00375 for (unsigned int i = 0 ; i < n ; ++i) 00376 intersections->push_back(ipts[i].pt); 00377 00378 return true; 00379 } 00380 00381 bool bodies::Box::containsPoint(const tf::Vector3 &p, bool verbose) const 00382 { 00383 /* if(verbose) 00384 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);*/ 00385 tf::Vector3 v = p - m_center; 00386 double pL = v.dot(m_normalL); 00387 00388 if (fabs(pL) > m_length2) 00389 return false; 00390 00391 double pW = v.dot(m_normalW); 00392 00393 if (fabs(pW) > m_width2) 00394 return false; 00395 00396 double pH = v.dot(m_normalH); 00397 00398 if (fabs(pH) > m_height2) 00399 return false; 00400 00401 return true; 00402 } 00403 00404 void bodies::Box::useDimensions(const shapes::Shape *shape) // (x, y, z) = (length, width, height) 00405 { 00406 const double *size = static_cast<const shapes::Box*>(shape)->size; 00407 m_length = size[0]; 00408 m_width = size[1]; 00409 m_height = size[2]; 00410 } 00411 00412 void bodies::Box::updateInternalData(void) 00413 { 00414 double s2 = m_scale / 2.0; 00415 m_length2 = m_length * s2 + m_padding; 00416 m_width2 = m_width * s2 + m_padding; 00417 m_height2 = m_height * s2 + m_padding; 00418 00419 m_center = m_pose.getOrigin(); 00420 00421 m_radius2 = m_length2 * m_length2 + m_width2 * m_width2 + m_height2 * m_height2; 00422 m_radiusB = sqrt(m_radius2); 00423 00424 const tf::Matrix3x3& basis = m_pose.getBasis(); 00425 m_normalL = basis.getColumn(0); 00426 m_normalW = basis.getColumn(1); 00427 m_normalH = basis.getColumn(2); 00428 00429 const tf::Vector3 tmp(m_normalL * m_length2 + m_normalW * m_width2 + m_normalH * m_height2); 00430 m_corner1 = m_center - tmp; 00431 m_corner2 = m_center + tmp; 00432 } 00433 00434 double bodies::Box::computeVolume(void) const 00435 { 00436 return 8.0 * m_length2 * m_width2 * m_height2; 00437 } 00438 00439 void bodies::Box::computeBoundingSphere(BoundingSphere &sphere) const 00440 { 00441 sphere.center = m_center; 00442 sphere.radius = m_radiusB; 00443 } 00444 00445 bool bodies::Box::intersectsRay(const tf::Vector3& origin, const tf::Vector3& dir, std::vector<tf::Vector3> *intersections, unsigned int count) const 00446 { 00447 if (distanceSQR(m_center, origin, dir) > m_radius2) return false; 00448 00449 double t_near = -INFINITY; 00450 double t_far = INFINITY; 00451 00452 for (int i = 0; i < 3; i++) 00453 { 00454 const tf::Vector3 &vN = i == 0 ? m_normalL : (i == 1 ? m_normalW : m_normalH); 00455 double dp = vN.dot(dir); 00456 00457 if (fabs(dp) > ZERO) 00458 { 00459 double t1 = vN.dot(m_corner1 - origin) / dp; 00460 double t2 = vN.dot(m_corner2 - origin) / dp; 00461 00462 if (t1 > t2) 00463 std::swap(t1, t2); 00464 00465 if (t1 > t_near) 00466 t_near = t1; 00467 00468 if (t2 < t_far) 00469 t_far = t2; 00470 00471 if (t_near > t_far) 00472 return false; 00473 00474 if (t_far < 0.0) 00475 return false; 00476 } 00477 else 00478 { 00479 if (i == 0) 00480 { 00481 if ((std::min(m_corner1.y(), m_corner2.y()) > origin.y() || 00482 std::max(m_corner1.y(), m_corner2.y()) < origin.y()) && 00483 (std::min(m_corner1.z(), m_corner2.z()) > origin.z() || 00484 std::max(m_corner1.z(), m_corner2.z()) < origin.z())) 00485 return false; 00486 } 00487 else 00488 { 00489 if (i == 1) 00490 { 00491 if ((std::min(m_corner1.x(), m_corner2.x()) > origin.x() || 00492 std::max(m_corner1.x(), m_corner2.x()) < origin.x()) && 00493 (std::min(m_corner1.z(), m_corner2.z()) > origin.z() || 00494 std::max(m_corner1.z(), m_corner2.z()) < origin.z())) 00495 return false; 00496 } 00497 else 00498 if ((std::min(m_corner1.x(), m_corner2.x()) > origin.x() || 00499 std::max(m_corner1.x(), m_corner2.x()) < origin.x()) && 00500 (std::min(m_corner1.y(), m_corner2.y()) > origin.y() || 00501 std::max(m_corner1.y(), m_corner2.y()) < origin.y())) 00502 return false; 00503 } 00504 } 00505 } 00506 00507 if (intersections) 00508 { 00509 if (t_far - t_near > ZERO) 00510 { 00511 intersections->push_back(t_near * dir + origin); 00512 if (count > 1) 00513 intersections->push_back(t_far * dir + origin); 00514 } 00515 else 00516 intersections->push_back(t_far * dir + origin); 00517 } 00518 00519 return true; 00520 } 00521 /* 00522 bool bodies::Mesh::containsPoint(const tf::Vector3 &p) const 00523 { 00524 // compute all intersections 00525 std::vector<tf::Vector3> pts; 00526 intersectsRay(p, tf::Vector3(0, 0, 1), &pts); 00527 00528 // if we have an odd number of intersections, we are inside 00529 return pts.size() % 2 == 1; 00530 } 00531 00532 namespace bodies 00533 { 00534 namespace detail 00535 { 00536 // callback for bullet 00537 class myTriangleCallback : public btTriangleCallback 00538 { 00539 public: 00540 00541 myTriangleCallback(bool keep_triangles) : keep_triangles_(keep_triangles) 00542 { 00543 found_intersections = false; 00544 } 00545 00546 virtual void processTriangle(tf::Vector3 *triangle, int partId, int triangleIndex) 00547 { 00548 found_intersections = true; 00549 if (keep_triangles_) 00550 triangles.push_back(btTriangleShape(triangle[0], triangle[1], triangle[2])); 00552 } 00553 00554 bool found_intersections; 00555 std::vector<btTriangleShape> triangles; 00556 00557 private: 00558 00559 bool keep_triangles_; 00560 }; 00561 00562 } 00563 } 00564 00565 bool bodies::Mesh::intersectsRay(const tf::Vector3& origin, const tf::Vector3& dir, std::vector<tf::Vector3> *intersections, unsigned int count) const 00566 { 00567 if (m_btMeshShape) 00568 { 00569 00570 if (distanceSQR(m_center, origin, dir) > m_radiusBSqr) return false; 00571 00572 // transform the ray into the coordinate frame of the mesh 00573 tf::Vector3 orig(m_iPose * origin); 00574 tf::Vector3 dr(m_iPose.getBasis() * dir); 00575 00576 // find intersection with AABB 00577 tfScalar maxT = 0.0; 00578 00579 if (fabs(dr.x()) > ZERO) 00580 { 00581 tfScalar t = (m_aabbMax.x() - orig.x()) / dr.x(); 00582 if (t < 0.0) 00583 t = (m_aabbMin.x() - orig.x()) / dr.x(); 00584 if (t > maxT) 00585 maxT = t; 00586 } 00587 if (fabs(dr.y()) > ZERO) 00588 { 00589 tfScalar t = (m_aabbMax.y() - orig.y()) / dr.y(); 00590 if (t < 0.0) 00591 t = (m_aabbMin.y() - orig.y()) / dr.y(); 00592 if (t > maxT) 00593 maxT = t; 00594 } 00595 if (fabs(dr.z()) > ZERO) 00596 { 00597 tfScalar t = (m_aabbMax.z() - orig.z()) / dr.z(); 00598 if (t < 0.0) 00599 t = (m_aabbMin.z() - orig.z()) / dr.z(); 00600 if (t > maxT) 00601 maxT = t; 00602 } 00603 00604 // the farthest point where we could have an intersection id orig + dr * maxT 00605 00606 detail::myTriangleCallback cb(intersections != NULL); 00607 m_btMeshShape->performRaycast(&cb, orig, orig + dr * maxT); 00608 if (cb.found_intersections) 00609 { 00610 if (intersections) 00611 { 00612 std::vector<detail::intersc> intpt; 00613 for (unsigned int i = 0 ; i < cb.triangles.size() ; ++i) 00614 { 00615 tf::Vector3 normal; 00616 cb.triangles[i].calcNormal(normal); 00617 tfScalar dv = normal.dot(dr); 00618 if (fabs(dv) > 1e-3) 00619 { 00620 double t = (normal.dot(cb.triangles[i].getVertexPtr(0)) - normal.dot(orig)) / dv; 00621 // here we use the input origin & direction, since the transform is linear 00622 detail::intersc ip(origin + dir * t, t); 00623 intpt.push_back(ip); 00624 } 00625 } 00626 std::sort(intpt.begin(), intpt.end(), detail::interscOrder()); 00627 const unsigned int n = count > 0 ? std::min<unsigned int>(count, intpt.size()) : intpt.size(); 00628 for (unsigned int i = 0 ; i < n ; ++i) 00629 intersections->push_back(intpt[i].pt); 00630 } 00631 return true; 00632 } 00633 else 00634 return false; 00635 } 00636 else 00637 return false; 00638 } 00639 00640 void bodies::Mesh::useDimensions(const shapes::Shape *shape) 00641 { 00642 const shapes::Mesh *mesh = static_cast<const shapes::Mesh*>(shape); 00643 if (m_btMeshShape) 00644 delete m_btMeshShape; 00645 if (m_btMesh) 00646 delete m_btMesh; 00647 00648 m_btMesh = new btTriangleMesh(); 00649 const unsigned int nt = mesh->triangleCount / 3; 00650 for (unsigned int i = 0 ; i < nt ; ++i) 00651 { 00652 const unsigned int i3 = i *3; 00653 const unsigned int v1 = 3 * mesh->triangles[i3]; 00654 const unsigned int v2 = 3 * mesh->triangles[i3 + 1]; 00655 const unsigned int v3 = 3 * mesh->triangles[i3 + 2]; 00656 m_btMesh->addTriangle(tf::Vector3(mesh->vertices[v1], mesh->vertices[v1 + 1], mesh->vertices[v1 + 2]), 00657 tf::Vector3(mesh->vertices[v2], mesh->vertices[v2 + 1], mesh->vertices[v2 + 2]), 00658 tf::Vector3(mesh->vertices[v3], mesh->vertices[v3 + 1], mesh->vertices[v3 + 2]), true); 00659 } 00660 00661 m_btMeshShape = new btBvhTriangleMeshShape(m_btMesh, true); 00662 } 00663 00664 void bodies::Mesh::updateInternalData(void) 00665 { 00666 if (m_btMeshShape) 00667 { 00668 m_btMeshShape->setLocalScaling(tf::Vector3(m_scale, m_scale, m_scale)); 00669 m_btMeshShape->setMargin(m_padding); 00670 } 00671 m_iPose = m_pose.inverse(); 00672 00673 tf::Transform id; 00674 id.setIdentity(); 00675 m_btMeshShape->getAabb(id, m_aabbMin, m_aabbMax); 00676 00677 tf::Vector3 d = (m_aabbMax - m_aabbMin) / 2.0; 00678 00679 m_center = m_pose.getOrigin() + d; 00680 m_radiusBSqr = d.length2(); 00681 m_radiusB = sqrt(m_radiusBSqr); 00682 00684 } 00685 00686 double bodies::Mesh::computeVolume(void) const 00687 { 00688 if (m_btMeshShape) 00689 { 00690 // approximation; volume of AABB at identity transform 00691 tf::Vector3 d = m_aabbMax - m_aabbMin; 00692 return d.x() * d.y() * d.z(); 00693 } 00694 else 00695 return 0.0; 00696 } 00697 00698 void bodies::Mesh::computeBoundingSphere(BoundingSphere &sphere) const 00699 { 00700 sphere.center = m_center; 00701 sphere.radius = m_radiusB; 00702 } 00703 00704 */ 00705 00706 bool bodies::ConvexMesh::containsPoint(const tf::Vector3 &p, bool verbose) const 00707 { 00708 if (m_boundingBox.containsPoint(p)) 00709 { 00710 tf::Vector3 ip(m_iPose * p); 00711 ip = m_meshCenter + (ip - m_meshCenter) * m_scale; 00712 return isPointInsidePlanes(ip); 00713 } 00714 else 00715 return false; 00716 } 00717 00718 void bodies::ConvexMesh::useDimensions(const shapes::Shape *shape) 00719 { 00720 const shapes::Mesh *mesh = static_cast<const shapes::Mesh*>(shape); 00721 00722 double maxX = -INFINITY, maxY = -INFINITY, maxZ = -INFINITY; 00723 double minX = INFINITY, minY = INFINITY, minZ = INFINITY; 00724 00725 for(unsigned int i = 0; i < mesh->vertexCount ; ++i) 00726 { 00727 double vx = mesh->vertices[3 * i ]; 00728 double vy = mesh->vertices[3 * i + 1]; 00729 double vz = mesh->vertices[3 * i + 2]; 00730 00731 if (maxX < vx) maxX = vx; 00732 if (maxY < vy) maxY = vy; 00733 if (maxZ < vz) maxZ = vz; 00734 00735 if (minX > vx) minX = vx; 00736 if (minY > vy) minY = vy; 00737 if (minZ > vz) minZ = vz; 00738 } 00739 00740 if (maxX < minX) maxX = minX = 0.0; 00741 if (maxY < minY) maxY = minY = 0.0; 00742 if (maxZ < minZ) maxZ = minZ = 0.0; 00743 00744 shapes::Box *box_shape = new shapes::Box(maxX - minX, maxY - minY, maxZ - minZ); 00745 m_boundingBox.setDimensions(box_shape); 00746 delete box_shape; 00747 00748 m_boxOffset.setValue((minX + maxX) / 2.0, (minY + maxY) / 2.0, (minZ + maxZ) / 2.0); 00749 00750 m_planes.clear(); 00751 m_triangles.clear(); 00752 m_vertices.clear(); 00753 m_meshRadiusB = 0.0; 00754 m_meshCenter.setValue(tfScalar(0), tfScalar(0), tfScalar(0)); 00755 00756 btVector3 *vertices = new btVector3[mesh->vertexCount]; 00757 for(unsigned int i = 0; i < mesh->vertexCount ; ++i) 00758 { 00759 vertices[i].setX(mesh->vertices[3 * i ]); 00760 vertices[i].setY(mesh->vertices[3 * i + 1]); 00761 vertices[i].setZ(mesh->vertices[3 * i + 2]); 00762 } 00763 00764 HullDesc hd(QF_TRIANGLES, mesh->vertexCount, vertices); 00765 HullResult hr; 00766 HullLibrary hl; 00767 if (hl.CreateConvexHull(hd, hr) == QE_OK) 00768 { 00769 // std::cout << "Convex hull has " << hr.m_OutputVertices.size() << " vertices (down from " << mesh->vertexCount << "), " << hr.mNumFaces << " faces" << std::endl; 00770 00771 m_vertices.reserve(hr.m_OutputVertices.size()); 00772 tf::Vector3 sum(0, 0, 0); 00773 00774 for (int j = 0 ; j < hr.m_OutputVertices.size() ; ++j) 00775 { 00776 m_vertices.push_back(hr.m_OutputVertices[j]); 00777 sum = sum + hr.m_OutputVertices[j]; 00778 } 00779 00780 m_meshCenter = sum / (double)(hr.m_OutputVertices.size()); 00781 for (unsigned int j = 0 ; j < m_vertices.size() ; ++j) 00782 { 00783 double dist = m_vertices[j].distance2(m_meshCenter); 00784 if (dist > m_meshRadiusB) 00785 m_meshRadiusB = dist; 00786 } 00787 m_meshRadiusB = sqrt(m_meshRadiusB); 00788 00789 m_triangles.reserve(hr.m_Indices.size()); 00790 for (unsigned int j = 0 ; j < hr.mNumFaces ; ++j) 00791 { 00792 const tf::Vector3 &p1 = hr.m_OutputVertices[hr.m_Indices[j * 3 ]]; 00793 const tf::Vector3 &p2 = hr.m_OutputVertices[hr.m_Indices[j * 3 + 1]]; 00794 const tf::Vector3 &p3 = hr.m_OutputVertices[hr.m_Indices[j * 3 + 2]]; 00795 00796 tf::Vector3 edge1 = (p2 - p1); 00797 tf::Vector3 edge2 = (p3 - p1); 00798 00799 edge1.normalize(); 00800 edge2.normalize(); 00801 00802 tf::Vector3 planeNormal = edge1.cross(edge2); 00803 00804 if (planeNormal.length2() > tfScalar(1e-6)) 00805 { 00806 planeNormal.normalize(); 00807 btVector4 planeEquation(planeNormal.getX(), planeNormal.getY(), planeNormal.getZ(), -planeNormal.dot(p1)); 00808 00809 unsigned int behindPlane = countVerticesBehindPlane(planeEquation); 00810 if (behindPlane > 0) 00811 { 00812 btVector4 planeEquation2(-planeEquation.getX(), -planeEquation.getY(), -planeEquation.getZ(), -planeEquation.getW()); 00813 unsigned int behindPlane2 = countVerticesBehindPlane(planeEquation2); 00814 if (behindPlane2 < behindPlane) 00815 { 00816 planeEquation.setValue(planeEquation2.getX(), planeEquation2.getY(), planeEquation2.getZ(), planeEquation2.getW()); 00817 behindPlane = behindPlane2; 00818 } 00819 } 00820 00821 // if (behindPlane > 0) 00822 // std::cerr << "Approximate plane: " << behindPlane << " of " << m_vertices.size() << " points are behind the plane" << std::endl; 00823 00824 m_planes.push_back(planeEquation); 00825 00826 m_triangles.push_back(hr.m_Indices[j * 3 + 0]); 00827 m_triangles.push_back(hr.m_Indices[j * 3 + 1]); 00828 m_triangles.push_back(hr.m_Indices[j * 3 + 2]); 00829 } 00830 } 00831 } 00832 else 00833 std::cerr << "Unable to compute convex hull."; 00834 00835 hl.ReleaseResult(hr); 00836 delete[] vertices; 00837 00838 } 00839 00840 void bodies::ConvexMesh::updateInternalData(void) 00841 { 00842 tf::Transform pose = m_pose; 00843 pose.setOrigin(m_pose * m_boxOffset); 00844 m_boundingBox.setPose(pose); 00845 m_boundingBox.setPadding(m_padding); 00846 m_boundingBox.setScale(m_scale); 00847 00848 m_iPose = m_pose.inverse(); 00849 m_center = m_pose * m_meshCenter; 00850 m_radiusB = m_meshRadiusB * m_scale + m_padding; 00851 m_radiusBSqr = m_radiusB * m_radiusB; 00852 00853 m_scaledVertices.resize(m_vertices.size()); 00854 for (unsigned int i = 0 ; i < m_vertices.size() ; ++i) 00855 { 00856 tf::Vector3 v(m_vertices[i] - m_meshCenter); 00857 tfScalar l = v.length(); 00858 m_scaledVertices[i] = m_meshCenter + v * (m_scale + (l > ZERO ? m_padding / l : 0.0)); 00859 } 00860 } 00861 00862 void bodies::ConvexMesh::computeBoundingSphere(BoundingSphere &sphere) const 00863 { 00864 sphere.center = m_center; 00865 sphere.radius = m_radiusB; 00866 } 00867 00868 bool bodies::ConvexMesh::isPointInsidePlanes(const tf::Vector3& point) const 00869 { 00870 unsigned int numplanes = m_planes.size(); 00871 for (unsigned int i = 0 ; i < numplanes ; ++i) 00872 { 00873 const btVector4& plane = m_planes[i]; 00874 tfScalar dist = plane.dot(point.asBt()) + plane.getW() - m_padding - btScalar(1e-6); 00875 if (dist > tfScalar(0)) 00876 return false; 00877 } 00878 return true; 00879 } 00880 00881 unsigned int bodies::ConvexMesh::countVerticesBehindPlane(const btVector4& planeNormal) const 00882 { 00883 unsigned int numvertices = m_vertices.size(); 00884 unsigned int result = 0; 00885 for (unsigned int i = 0 ; i < numvertices ; ++i) 00886 { 00887 tfScalar dist = planeNormal.dot(m_vertices[i].asBt()) + planeNormal.getW() - tfScalar(1e-6); 00888 if (dist > tfScalar(0)) 00889 result++; 00890 } 00891 return result; 00892 } 00893 00894 double bodies::ConvexMesh::computeVolume(void) const 00895 { 00896 double volume = 0.0; 00897 for (unsigned int i = 0 ; i < m_triangles.size() / 3 ; ++i) 00898 { 00899 const tf::Vector3 &v1 = m_vertices[m_triangles[3*i + 0]]; 00900 const tf::Vector3 &v2 = m_vertices[m_triangles[3*i + 1]]; 00901 const tf::Vector3 &v3 = m_vertices[m_triangles[3*i + 2]]; 00902 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(); 00903 } 00904 return fabs(volume)/6.0; 00905 } 00906 00907 bool bodies::ConvexMesh::intersectsRay(const tf::Vector3& origin, const tf::Vector3& dir, std::vector<tf::Vector3> *intersections, unsigned int count) const 00908 { 00909 if (distanceSQR(m_center, origin, dir) > m_radiusBSqr) return false; 00910 if (!m_boundingBox.intersectsRay(origin, dir)) return false; 00911 00912 // transform the ray into the coordinate frame of the mesh 00913 tf::Vector3 orig(m_iPose * origin); 00914 tf::Vector3 dr(m_iPose.getBasis() * dir); 00915 00916 std::vector<detail::intersc> ipts; 00917 00918 bool result = false; 00919 00920 // for each triangle 00921 const unsigned int nt = m_triangles.size() / 3; 00922 for (unsigned int i = 0 ; i < nt ; ++i) 00923 { 00924 tfScalar tmp = m_planes[i].dot(dr.asBt()); 00925 if (fabs(tmp) > ZERO) 00926 { 00927 double t = -(m_planes[i].dot(orig.asBt()) + m_planes[i].getW()) / tmp; 00928 if (t > 0.0) 00929 { 00930 const int i3 = 3 * i; 00931 const int v1 = m_triangles[i3 + 0]; 00932 const int v2 = m_triangles[i3 + 1]; 00933 const int v3 = m_triangles[i3 + 2]; 00934 00935 const tf::Vector3 &a = m_scaledVertices[v1]; 00936 const tf::Vector3 &b = m_scaledVertices[v2]; 00937 const tf::Vector3 &c = m_scaledVertices[v3]; 00938 00939 tf::Vector3 cb(c - b); 00940 tf::Vector3 ab(a - b); 00941 00942 // intersection of the plane defined by the triangle and the ray 00943 tf::Vector3 P(orig + dr * t); 00944 00945 // check if it is inside the triangle 00946 tf::Vector3 pb(P - b); 00947 tf::Vector3 c1(cb.cross(pb)); 00948 tf::Vector3 c2(cb.cross(ab)); 00949 if (c1.dot(c2) < 0.0) 00950 continue; 00951 00952 tf::Vector3 ca(c - a); 00953 tf::Vector3 pa(P - a); 00954 tf::Vector3 ba(-ab); 00955 00956 c1 = ca.cross(pa); 00957 c2 = ca.cross(ba); 00958 if (c1.dot(c2) < 0.0) 00959 continue; 00960 00961 c1 = ba.cross(pa); 00962 c2 = ba.cross(ca); 00963 00964 if (c1.dot(c2) < 0.0) 00965 continue; 00966 00967 result = true; 00968 if (intersections) 00969 { 00970 detail::intersc ip(origin + dir * t, t); 00971 ipts.push_back(ip); 00972 } 00973 else 00974 break; 00975 } 00976 } 00977 } 00978 00979 if (intersections) 00980 { 00981 std::sort(ipts.begin(), ipts.end(), detail::interscOrder()); 00982 const unsigned int n = count > 0 ? std::min<unsigned int>(count, ipts.size()) : ipts.size(); 00983 for (unsigned int i = 0 ; i < n ; ++i) 00984 intersections->push_back(ipts[i].pt); 00985 } 00986 00987 return result; 00988 }