Changeset 8393 for code/trunk/src/external/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
- Timestamp:
- May 3, 2011, 5:07:42 AM (13 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
code/trunk/src/external/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
r8351 r8393 29 29 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" 30 30 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" 31 #include "BulletCollision/BroadphaseCollision/btDbvt.h" 31 32 #include "LinearMath/btAabbUtil2.h" 32 33 #include "LinearMath/btQuickprof.h" 33 34 #include "LinearMath/btStackAlloc.h" 34 35 #include "LinearMath/btSerializer.h" 36 #include "BulletCollision/CollisionShapes/btConvexPolyhedron.h" 37 38 //#define DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION 39 35 40 36 41 //#define USE_BRUTEFORCE_RAYBROADPHASE 1 … … 151 156 maxAabb += contactThreshold; 152 157 158 if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY) 159 { 160 btVector3 minAabb2,maxAabb2; 161 colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2); 162 minAabb2 -= contactThreshold; 163 maxAabb2 += contactThreshold; 164 minAabb.setMin(minAabb2); 165 maxAabb.setMax(maxAabb2); 166 } 167 153 168 btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache; 154 169 … … 421 436 } else { 422 437 // BT_PROFILE("rayTestCompound"); 423 ///@todo: use AABB tree or other BVH acceleration structure, see btDbvt424 438 if (collisionShape->isCompound()) 425 439 { 440 struct LocalInfoAdder2 : public RayResultCallback 441 { 442 RayResultCallback* m_userCallback; 443 int m_i; 444 445 LocalInfoAdder2 (int i, RayResultCallback *user) 446 : m_userCallback(user), m_i(i) 447 { 448 m_closestHitFraction = m_userCallback->m_closestHitFraction; 449 } 450 virtual bool needsCollision(btBroadphaseProxy* p) const 451 { 452 return m_userCallback->needsCollision(p); 453 } 454 455 virtual btScalar addSingleResult (btCollisionWorld::LocalRayResult &r, bool b) 456 { 457 btCollisionWorld::LocalShapeInfo shapeInfo; 458 shapeInfo.m_shapePart = -1; 459 shapeInfo.m_triangleIndex = m_i; 460 if (r.m_localShapeInfo == NULL) 461 r.m_localShapeInfo = &shapeInfo; 462 463 const btScalar result = m_userCallback->addSingleResult(r, b); 464 m_closestHitFraction = m_userCallback->m_closestHitFraction; 465 return result; 466 } 467 }; 468 469 struct RayTester : btDbvt::ICollide 470 { 471 btCollisionObject* m_collisionObject; 472 const btCompoundShape* m_compoundShape; 473 const btTransform& m_colObjWorldTransform; 474 const btTransform& m_rayFromTrans; 475 const btTransform& m_rayToTrans; 476 RayResultCallback& m_resultCallback; 477 478 RayTester(btCollisionObject* collisionObject, 479 const btCompoundShape* compoundShape, 480 const btTransform& colObjWorldTransform, 481 const btTransform& rayFromTrans, 482 const btTransform& rayToTrans, 483 RayResultCallback& resultCallback): 484 m_collisionObject(collisionObject), 485 m_compoundShape(compoundShape), 486 m_colObjWorldTransform(colObjWorldTransform), 487 m_rayFromTrans(rayFromTrans), 488 m_rayToTrans(rayToTrans), 489 m_resultCallback(resultCallback) 490 { 491 492 } 493 494 void Process(int i) 495 { 496 const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(i); 497 const btTransform& childTrans = m_compoundShape->getChildTransform(i); 498 btTransform childWorldTrans = m_colObjWorldTransform * childTrans; 499 500 // replace collision shape so that callback can determine the triangle 501 btCollisionShape* saveCollisionShape = m_collisionObject->getCollisionShape(); 502 m_collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape); 503 504 LocalInfoAdder2 my_cb(i, &m_resultCallback); 505 506 rayTestSingle( 507 m_rayFromTrans, 508 m_rayToTrans, 509 m_collisionObject, 510 childCollisionShape, 511 childWorldTrans, 512 my_cb); 513 514 // restore 515 m_collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape); 516 } 517 518 void Process(const btDbvtNode* leaf) 519 { 520 Process(leaf->dataAsInt); 521 } 522 }; 523 426 524 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape); 427 int i=0; 428 for (i=0;i<compoundShape->getNumChildShapes();i++) 429 { 430 btTransform childTrans = compoundShape->getChildTransform(i); 431 const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i); 432 btTransform childWorldTrans = colObjWorldTransform * childTrans; 433 // replace collision shape so that callback can determine the triangle 434 btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape(); 435 collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape); 436 struct LocalInfoAdder2 : public RayResultCallback { 437 RayResultCallback* m_userCallback; 438 int m_i; 439 LocalInfoAdder2 (int i, RayResultCallback *user) 440 : m_userCallback(user), 441 m_i(i) 442 { 443 m_closestHitFraction = m_userCallback->m_closestHitFraction; 444 } 445 virtual btScalar addSingleResult (btCollisionWorld::LocalRayResult &r, bool b) 446 { 447 btCollisionWorld::LocalShapeInfo shapeInfo; 448 shapeInfo.m_shapePart = -1; 449 shapeInfo.m_triangleIndex = m_i; 450 if (r.m_localShapeInfo == NULL) 451 r.m_localShapeInfo = &shapeInfo; 452 453 const btScalar result = m_userCallback->addSingleResult(r, b); 454 m_closestHitFraction = m_userCallback->m_closestHitFraction; 455 return result; 456 } 457 }; 458 459 LocalInfoAdder2 my_cb(i, &resultCallback); 460 461 rayTestSingle(rayFromTrans,rayToTrans, 462 collisionObject, 463 childCollisionShape, 464 childWorldTrans, 465 my_cb); 466 // restore 467 collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape); 525 const btDbvt* dbvt = compoundShape->getDynamicAabbTree(); 526 527 528 RayTester rayCB( 529 collisionObject, 530 compoundShape, 531 colObjWorldTransform, 532 rayFromTrans, 533 rayToTrans, 534 resultCallback); 535 #ifndef DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION 536 if (dbvt) 537 { 538 btVector3 localRayFrom = colObjWorldTransform.inverseTimes(rayFromTrans).getOrigin(); 539 btVector3 localRayTo = colObjWorldTransform.inverseTimes(rayToTrans).getOrigin(); 540 btDbvt::rayTest(dbvt->m_root, localRayFrom , localRayTo, rayCB); 541 } 542 else 543 #endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION 544 { 545 for (int i = 0, n = compoundShape->getNumChildShapes(); i < n; ++i) 546 { 547 rayCB.Process(i); 548 } 468 549 } 469 550 } … … 577 658 BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,triangleMesh, colObjWorldTransform); 578 659 tccb.m_hitFraction = resultCallback.m_closestHitFraction; 660 tccb.m_allowedPenetration = allowedPenetration; 579 661 btVector3 boxMinLocal, boxMaxLocal; 580 662 castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal); … … 582 664 } else 583 665 { 584 //BT_PROFILE("convexSweepConcave"); 585 btConcaveShape* concaveShape = (btConcaveShape*)collisionShape; 586 btTransform worldTocollisionObject = colObjWorldTransform.inverse(); 587 btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin(); 588 btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin(); 589 // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation 590 btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis()); 591 592 //ConvexCast::CastResult 593 struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback 594 { 595 btCollisionWorld::ConvexResultCallback* m_resultCallback; 596 btCollisionObject* m_collisionObject; 597 btConcaveShape* m_triangleMesh; 598 599 BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to, 600 btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& triangleToWorld): 601 btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()), 602 m_resultCallback(resultCallback), 603 m_collisionObject(collisionObject), 604 m_triangleMesh(triangleMesh) 605 { 666 if (collisionShape->getShapeType()==STATIC_PLANE_PROXYTYPE) 667 { 668 btConvexCast::CastResult castResult; 669 castResult.m_allowedPenetration = allowedPenetration; 670 castResult.m_fraction = resultCallback.m_closestHitFraction; 671 btStaticPlaneShape* planeShape = (btStaticPlaneShape*) collisionShape; 672 btContinuousConvexCollision convexCaster1(castShape,planeShape); 673 btConvexCast* castPtr = &convexCaster1; 674 675 if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) 676 { 677 //add hit 678 if (castResult.m_normal.length2() > btScalar(0.0001)) 679 { 680 if (castResult.m_fraction < resultCallback.m_closestHitFraction) 681 { 682 castResult.m_normal.normalize(); 683 btCollisionWorld::LocalConvexResult localConvexResult 684 ( 685 collisionObject, 686 0, 687 castResult.m_normal, 688 castResult.m_hitPoint, 689 castResult.m_fraction 690 ); 691 692 bool normalInWorldSpace = true; 693 resultCallback.addSingleResult(localConvexResult, normalInWorldSpace); 694 } 695 } 606 696 } 607 697 608 609 virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex ) 610 { 611 btCollisionWorld::LocalShapeInfo shapeInfo; 612 shapeInfo.m_shapePart = partId; 613 shapeInfo.m_triangleIndex = triangleIndex; 614 if (hitFraction <= m_resultCallback->m_closestHitFraction) 698 } else 699 { 700 //BT_PROFILE("convexSweepConcave"); 701 btConcaveShape* concaveShape = (btConcaveShape*)collisionShape; 702 btTransform worldTocollisionObject = colObjWorldTransform.inverse(); 703 btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin(); 704 btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin(); 705 // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation 706 btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis()); 707 708 //ConvexCast::CastResult 709 struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback 710 { 711 btCollisionWorld::ConvexResultCallback* m_resultCallback; 712 btCollisionObject* m_collisionObject; 713 btConcaveShape* m_triangleMesh; 714 715 BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to, 716 btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& triangleToWorld): 717 btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()), 718 m_resultCallback(resultCallback), 719 m_collisionObject(collisionObject), 720 m_triangleMesh(triangleMesh) 615 721 { 616 617 btCollisionWorld::LocalConvexResult convexResult618 (m_collisionObject,619 &shapeInfo,620 hitNormalLocal,621 hitPointLocal,622 hitFraction);623 624 bool normalInWorldSpace = false;625 626 return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);627 722 } 628 return hitFraction; 629 } 630 631 }; 632 633 BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,concaveShape, colObjWorldTransform); 634 tccb.m_hitFraction = resultCallback.m_closestHitFraction; 635 btVector3 boxMinLocal, boxMaxLocal; 636 castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal); 637 638 btVector3 rayAabbMinLocal = convexFromLocal; 639 rayAabbMinLocal.setMin(convexToLocal); 640 btVector3 rayAabbMaxLocal = convexFromLocal; 641 rayAabbMaxLocal.setMax(convexToLocal); 642 rayAabbMinLocal += boxMinLocal; 643 rayAabbMaxLocal += boxMaxLocal; 644 concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal); 723 724 725 virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex ) 726 { 727 btCollisionWorld::LocalShapeInfo shapeInfo; 728 shapeInfo.m_shapePart = partId; 729 shapeInfo.m_triangleIndex = triangleIndex; 730 if (hitFraction <= m_resultCallback->m_closestHitFraction) 731 { 732 733 btCollisionWorld::LocalConvexResult convexResult 734 (m_collisionObject, 735 &shapeInfo, 736 hitNormalLocal, 737 hitPointLocal, 738 hitFraction); 739 740 bool normalInWorldSpace = false; 741 742 return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace); 743 } 744 return hitFraction; 745 } 746 747 }; 748 749 BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,concaveShape, colObjWorldTransform); 750 tccb.m_hitFraction = resultCallback.m_closestHitFraction; 751 tccb.m_allowedPenetration = allowedPenetration; 752 btVector3 boxMinLocal, boxMaxLocal; 753 castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal); 754 755 btVector3 rayAabbMinLocal = convexFromLocal; 756 rayAabbMinLocal.setMin(convexToLocal); 757 btVector3 rayAabbMaxLocal = convexFromLocal; 758 rayAabbMaxLocal.setMax(convexToLocal); 759 rayAabbMinLocal += boxMinLocal; 760 rayAabbMaxLocal += boxMaxLocal; 761 concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal); 762 } 645 763 } 646 764 } else { … … 667 785 { 668 786 m_closestHitFraction = m_userCallback->m_closestHitFraction; 787 } 788 virtual bool needsCollision(btBroadphaseProxy* p) const 789 { 790 return m_userCallback->needsCollision(p); 669 791 } 670 792 virtual btScalar addSingleResult (btCollisionWorld::LocalConvexResult& r, bool b) … … 1152 1274 1153 1275 int upAxis = capsuleShape->getUpAxis(); 1154 1155 1156 btVector3 capStart(0.f,0.f,0.f); 1157 capStart[upAxis] = -halfHeight; 1158 1159 btVector3 capEnd(0.f,0.f,0.f); 1160 capEnd[upAxis] = halfHeight; 1161 1162 // Draw the ends 1163 { 1164 1165 btTransform childTransform = worldTransform; 1166 childTransform.getOrigin() = worldTransform * capStart; 1167 getDebugDrawer()->drawSphere(radius, childTransform, color); 1168 } 1169 1170 { 1171 btTransform childTransform = worldTransform; 1172 childTransform.getOrigin() = worldTransform * capEnd; 1173 getDebugDrawer()->drawSphere(radius, childTransform, color); 1174 } 1175 1176 // Draw some additional lines 1177 btVector3 start = worldTransform.getOrigin(); 1178 1179 1180 capStart[(upAxis+1)%3] = radius; 1181 capEnd[(upAxis+1)%3] = radius; 1182 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color); 1183 capStart[(upAxis+1)%3] = -radius; 1184 capEnd[(upAxis+1)%3] = -radius; 1185 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color); 1186 1187 capStart[(upAxis+1)%3] = 0.f; 1188 capEnd[(upAxis+1)%3] = 0.f; 1189 1190 capStart[(upAxis+2)%3] = radius; 1191 capEnd[(upAxis+2)%3] = radius; 1192 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color); 1193 capStart[(upAxis+2)%3] = -radius; 1194 capEnd[(upAxis+2)%3] = -radius; 1195 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color); 1196 1197 1276 getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, color); 1198 1277 break; 1199 1278 } … … 1203 1282 btScalar radius = coneShape->getRadius();//+coneShape->getMargin(); 1204 1283 btScalar height = coneShape->getHeight();//+coneShape->getMargin(); 1205 btVector3 start = worldTransform.getOrigin();1206 1284 1207 1285 int upAxis= coneShape->getConeUpIndex(); 1208 1209 1210 btVector3 offsetHeight(0,0,0); 1211 offsetHeight[upAxis] = height * btScalar(0.5); 1212 btVector3 offsetRadius(0,0,0); 1213 offsetRadius[(upAxis+1)%3] = radius; 1214 btVector3 offset2Radius(0,0,0); 1215 offset2Radius[(upAxis+2)%3] = radius; 1216 1217 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color); 1218 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color); 1219 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color); 1220 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color); 1221 1222 // Drawing the base of the cone 1223 btVector3 yaxis(0,0,0); 1224 yaxis[upAxis] = btScalar(1.0); 1225 btVector3 xaxis(0,0,0); 1226 xaxis[(upAxis+1)%3] = btScalar(1.0); 1227 getDebugDrawer()->drawArc(start-worldTransform.getBasis()*(offsetHeight),worldTransform.getBasis()*yaxis,worldTransform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,10.0); 1228 break; 1286 getDebugDrawer()->drawCone(radius, height, upAxis, worldTransform, color); 1287 break; 1229 1288 1230 1289 } … … 1235 1294 btScalar radius = cylinder->getRadius(); 1236 1295 btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis]; 1237 btVector3 start = worldTransform.getOrigin(); 1238 btVector3 offsetHeight(0,0,0); 1239 offsetHeight[upAxis] = halfHeight; 1240 btVector3 offsetRadius(0,0,0); 1241 offsetRadius[(upAxis+1)%3] = radius; 1242 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color); 1243 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color); 1244 1245 // Drawing top and bottom caps of the cylinder 1246 btVector3 yaxis(0,0,0); 1247 yaxis[upAxis] = btScalar(1.0); 1248 btVector3 xaxis(0,0,0); 1249 xaxis[(upAxis+1)%3] = btScalar(1.0); 1250 getDebugDrawer()->drawArc(start-worldTransform.getBasis()*(offsetHeight),worldTransform.getBasis()*yaxis,worldTransform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,btScalar(10.0)); 1251 getDebugDrawer()->drawArc(start+worldTransform.getBasis()*(offsetHeight),worldTransform.getBasis()*yaxis,worldTransform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,btScalar(10.0)); 1296 getDebugDrawer()->drawCylinder(radius, halfHeight, upAxis, worldTransform, color); 1252 1297 break; 1253 1298 } … … 1258 1303 btScalar planeConst = staticPlaneShape->getPlaneConstant(); 1259 1304 const btVector3& planeNormal = staticPlaneShape->getPlaneNormal(); 1260 btVector3 planeOrigin = planeNormal * planeConst; 1261 btVector3 vec0,vec1; 1262 btPlaneSpace1(planeNormal,vec0,vec1); 1263 btScalar vecLen = 100.f; 1264 btVector3 pt0 = planeOrigin + vec0*vecLen; 1265 btVector3 pt1 = planeOrigin - vec0*vecLen; 1266 btVector3 pt2 = planeOrigin + vec1*vecLen; 1267 btVector3 pt3 = planeOrigin - vec1*vecLen; 1268 getDebugDrawer()->drawLine(worldTransform*pt0,worldTransform*pt1,color); 1269 getDebugDrawer()->drawLine(worldTransform*pt2,worldTransform*pt3,color); 1305 getDebugDrawer()->drawPlane(planeNormal, planeConst,worldTransform, color); 1270 1306 break; 1271 1307 … … 1305 1341 1306 1342 int i; 1307 for (i=0;i<polyshape->getNumEdges();i++) 1308 { 1309 btVector3 a,b; 1310 polyshape->getEdge(i,a,b); 1311 btVector3 wa = worldTransform * a; 1312 btVector3 wb = worldTransform * b; 1313 getDebugDrawer()->drawLine(wa,wb,color); 1314 1343 if (polyshape->getConvexPolyhedron()) 1344 { 1345 const btConvexPolyhedron* poly = polyshape->getConvexPolyhedron(); 1346 for (i=0;i<poly->m_faces.size();i++) 1347 { 1348 btVector3 centroid(0,0,0); 1349 int numVerts = poly->m_faces[i].m_indices.size(); 1350 if (numVerts) 1351 { 1352 int lastV = poly->m_faces[i].m_indices[numVerts-1]; 1353 for (int v=0;v<poly->m_faces[i].m_indices.size();v++) 1354 { 1355 int curVert = poly->m_faces[i].m_indices[v]; 1356 centroid+=poly->m_vertices[curVert]; 1357 getDebugDrawer()->drawLine(worldTransform*poly->m_vertices[lastV],worldTransform*poly->m_vertices[curVert],color); 1358 lastV = curVert; 1359 } 1360 } 1361 centroid*= 1./btScalar(numVerts); 1362 1363 btVector3 normalColor(1,1,0); 1364 btVector3 faceNormal(poly->m_faces[i].m_plane[0],poly->m_faces[i].m_plane[1],poly->m_faces[i].m_plane[2]); 1365 getDebugDrawer()->drawLine(worldTransform*centroid,worldTransform*(centroid+faceNormal),normalColor); 1366 1367 1368 } 1369 1370 1371 } else 1372 { 1373 for (i=0;i<polyshape->getNumEdges();i++) 1374 { 1375 btVector3 a,b; 1376 polyshape->getEdge(i,a,b); 1377 btVector3 wa = worldTransform * a; 1378 btVector3 wb = worldTransform * b; 1379 getDebugDrawer()->drawLine(wa,wb,color); 1380 } 1315 1381 } 1316 1382 … … 1381 1447 btVector3 colorvec(1,0,0); 1382 1448 colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); 1449 btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold); 1450 minAabb -= contactThreshold; 1451 maxAabb += contactThreshold; 1452 1453 btVector3 minAabb2,maxAabb2; 1454 1455 if(colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY) 1456 { 1457 colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2); 1458 minAabb2 -= contactThreshold; 1459 maxAabb2 += contactThreshold; 1460 minAabb.setMin(minAabb2); 1461 maxAabb.setMax(maxAabb2); 1462 } 1463 1383 1464 m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec); 1384 1465 }
Note: See TracChangeset
for help on using the changeset viewer.