00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00037 #include "pr2_navigation_self_filter/bodies.h"
00038 #include <LinearMath/btConvexHull.h>
00039
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(btScalar(0), btScalar(0), btScalar(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 btVector3 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 btVector3& p, const btVector3& origin, const btVector3& dir)
00109 {
00110 btVector3 a = p - origin;
00111 double d = dir.dot(a);
00112 return a.length2() - d * d;
00113 }
00114
00115 namespace detail
00116 {
00117
00118
00119 struct intersc
00120 {
00121 intersc(const btVector3 &_pt, const double _tm) : pt(_pt), time(_tm) {}
00122
00123 btVector3 pt;
00124 double time;
00125 };
00126
00127
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 btVector3 &p, bool verbose) const
00139 {
00140 return (m_center - p).length2() < m_radius2;
00141 }
00142
00143 void bodies::Sphere::useDimensions(const shapes::Shape *shape)
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 btVector3& origin, const btVector3& dir, std::vector<btVector3> *intersections, unsigned int count) const
00167 {
00168 if (distanceSQR(m_center, origin, dir) > m_radius2) return false;
00169
00170 bool result = false;
00171
00172 btVector3 cp = origin - m_center;
00173 double dpcpv = cp.dot(dir);
00174
00175 btVector3 w = cp - dpcpv * dir;
00176 btVector3 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 btVector3 A = Q - w;
00195 btVector3 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 btVector3 &p, bool verbose) const
00223 {
00224 btVector3 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)
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 btMatrix3x3& 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 btVector3& origin, const btVector3& dir, std::vector<btVector3> *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
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 btVector3 p1(origin + dir * t1);
00294 btVector3 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 btVector3 p2(origin + dir * t2);
00310 btVector3 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
00326 btVector3 VD(m_normalH.cross(dir));
00327 btVector3 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 btVector3 p1(origin + dir * t1);
00342 btVector3 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 btVector3 p2(origin + dir * t2);
00357 btVector3 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 btVector3 &p, bool verbose) const
00382 {
00383
00384
00385 btVector3 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)
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 btMatrix3x3& 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 btVector3 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 btVector3& origin, const btVector3& dir, std::vector<btVector3> *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 btVector3 &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
00523
00524
00525
00526
00527
00528
00529
00530
00531
00532
00533
00534
00535
00536
00537
00538
00539
00540
00541
00542
00543
00544
00545
00546
00547
00548
00549
00550
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573
00574
00575
00576
00577
00578
00579
00580
00581
00582
00583
00584
00585
00586
00587
00588
00589
00590
00591
00592
00593
00594
00595
00596
00597
00598
00599
00600
00601
00602
00603
00604
00605
00606
00607
00608
00609
00610
00611
00612
00613
00614
00615
00616
00617
00618
00619
00620
00621
00622
00623
00624
00625
00626
00627
00628
00629
00630
00631
00632
00633
00634
00635
00636
00637
00638
00639
00640
00641
00642
00643
00644
00645
00646
00647
00648
00649
00650
00651
00652
00653
00654
00655
00656
00657
00658
00659
00660
00661
00662
00663
00664
00665
00666
00667
00668
00669
00670
00671
00672
00673
00674
00675
00676
00677
00678
00679
00680
00681
00682
00684
00685
00686
00687
00688
00689
00690
00691
00692
00693
00694
00695
00696
00697
00698
00699
00700
00701
00702
00703
00704
00705
00706 bool bodies::ConvexMesh::containsPoint(const btVector3 &p, bool verbose) const
00707 {
00708 if (m_boundingBox.containsPoint(p))
00709 {
00710 btVector3 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(btScalar(0), btScalar(0), btScalar(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
00770
00771 m_vertices.reserve(hr.m_OutputVertices.size());
00772 btVector3 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 btVector3 &p1 = hr.m_OutputVertices[hr.m_Indices[j * 3 ]];
00793 const btVector3 &p2 = hr.m_OutputVertices[hr.m_Indices[j * 3 + 1]];
00794 const btVector3 &p3 = hr.m_OutputVertices[hr.m_Indices[j * 3 + 2]];
00795
00796 btVector3 edge1 = (p2 - p1);
00797 btVector3 edge2 = (p3 - p1);
00798
00799 edge1.normalize();
00800 edge2.normalize();
00801
00802 btVector3 planeNormal = edge1.cross(edge2);
00803
00804 if (planeNormal.length2() > btScalar(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
00822
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 btTransform 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 btVector3 v(m_vertices[i] - m_meshCenter);
00857 btScalar 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 btVector3& 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 btScalar dist = plane.dot(point) + plane.getW() - m_padding - btScalar(1e-6);
00875 if (dist > btScalar(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 btScalar dist = planeNormal.dot(m_vertices[i]) + planeNormal.getW() - btScalar(1e-6);
00888 if (dist > btScalar(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 btVector3 &v1 = m_vertices[m_triangles[3*i + 0]];
00900 const btVector3 &v2 = m_vertices[m_triangles[3*i + 1]];
00901 const btVector3 &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 btVector3& origin, const btVector3& dir, std::vector<btVector3> *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
00913 btVector3 orig(m_iPose * origin);
00914 btVector3 dr(m_iPose.getBasis() * dir);
00915
00916 std::vector<detail::intersc> ipts;
00917
00918 bool result = false;
00919
00920
00921 const unsigned int nt = m_triangles.size() / 3;
00922 for (unsigned int i = 0 ; i < nt ; ++i)
00923 {
00924 btScalar tmp = m_planes[i].dot(dr);
00925 if (fabs(tmp) > ZERO)
00926 {
00927 double t = -(m_planes[i].dot(orig) + 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 btVector3 &a = m_scaledVertices[v1];
00936 const btVector3 &b = m_scaledVertices[v2];
00937 const btVector3 &c = m_scaledVertices[v3];
00938
00939 btVector3 cb(c - b);
00940 btVector3 ab(a - b);
00941
00942
00943 btVector3 P(orig + dr * t);
00944
00945
00946 btVector3 pb(P - b);
00947 btVector3 c1(cb.cross(pb));
00948 btVector3 c2(cb.cross(ab));
00949 if (c1.dot(c2) < 0.0)
00950 continue;
00951
00952 btVector3 ca(c - a);
00953 btVector3 pa(P - a);
00954 btVector3 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 }