diff --git a/data/cube_small.urdf b/data/cube_small.urdf index 0bf03f5aa..299ded801 100644 --- a/data/cube_small.urdf +++ b/data/cube_small.urdf @@ -3,7 +3,6 @@ - diff --git a/data/gripper/meshes/l_gripper_tip_scaled.stl b/data/gripper/meshes/l_gripper_tip_scaled.stl index 94fb02462..e38df0ad9 100644 Binary files a/data/gripper/meshes/l_gripper_tip_scaled.stl and b/data/gripper/meshes/l_gripper_tip_scaled.stl differ diff --git a/data/jenga/jenga.mtl b/data/jenga/jenga.mtl new file mode 100644 index 000000000..a89fe6ca2 --- /dev/null +++ b/data/jenga/jenga.mtl @@ -0,0 +1,16 @@ +newmtl jenga + Ns 10.0000 + Ni 1.5000 + d 1.0000 + Tr 0.0000 + Tf 1.0000 1.0000 1.0000 + illum 2 + Ka 0.0000 0.0000 0.0000 + Kd 0.5880 0.5880 0.5880 + Ks 0.0000 0.0000 0.0000 + Ke 0.0000 0.0000 0.0000 + map_Ka jenga.tga + map_Kd jenga.png + + + diff --git a/data/jenga/jenga.obj b/data/jenga/jenga.obj new file mode 100644 index 000000000..b90863be1 --- /dev/null +++ b/data/jenga/jenga.obj @@ -0,0 +1,113 @@ +# jenga.obj +# + +o jenga +mtllib jenga.mtl + +v -0.5 -0.5 0.5 +v 0.5 -0.5 0.5 +v 0.5 0.5 0.5 +v -0.5 0.5 0.5 + +v -0.5 -0.5 -0.5 +v 0.5 -0.5 -0.5 +v 0.5 0.5 -0.5 +v -0.5 0.5 -0.5 + +v -0.5 -0.5 -0.5 +v -0.5 0.5 -0.5 +v -0.5 0.5 0.5 +v -0.5 -0.5 0.5 + +v 0.5 -0.5 -0.5 +v 0.5 0.5 -0.5 +v 0.5 0.5 0.5 +v 0.5 -0.5 0.5 +v -0.5 -0.5 -0.5 +v -0.5 -0.5 0.5 +v 0.5 -0.5 0.5 +v 0.5 -0.5 -0.5 +v -0.5 0.5 -0.5 +v -0.5 0.5 0.5 +v 0.5 0.5 0.5 +v 0.5 0.5 -0.5 + +vt 0 1 +vt 0 0.75 +vt 0.25 0.75 +vt 0.25 1 + +vt 0.25 0.5 +vt 0.25 0.75 +vt 0.5 0.75 +vt 0.5 0.5 + +vt 1 0.75 +vt 0.75 0.75 +vt 0.75 1 +vt 1 1 + +vt 0.25 0.75 +vt 0.5 0.75 +vt 0.5 1 +vt 0.25 1 + +vt 0 0.5 +vt 0 0.75 +vt 0.25 0.75 +vt 0.25 0.5 + + +vt 0.75 0.75 +vt 0.75 1 +vt 0.5 1 +vt 0.5 0.75 + +vn 0 0 1 +vn 0 0 1 +vn 0 0 1 +vn 0 0 1 +vn 0 0 -1 +vn 0 0 -1 +vn 0 0 -1 +vn 0 0 -1 +vn -1 0 0 +vn -1 0 0 +vn -1 0 0 +vn -1 0 0 +vn 1 0 0 +vn 1 0 0 +vn 1 0 0 +vn 1 0 0 +vn 0 -1 0 +vn 0 -1 0 +vn 0 -1 0 +vn 0 -1 0 +vn 0 1 0 +vn 0 1 0 +vn 0 1 0 +vn 0 1 0 + + +g jenga +usemtl jenga +s 1 +f 1/1/1 2/2/2 3/3/3 +f 1/1/1 3/3/3 4/4/4 +s 2 +f 7/7/7 6/6/6 5/5/5 +f 8/8/8 7/7/7 5/5/5 +s 3 +f 11/11/11 10/10/10 9/9/9 +f 12/12/12 11/11/11 9/9/9 +s 4 +f 13/13/13 14/14/14 15/15/15 +f 13/13/13 15/15/15 16/16/16 +s 5 +f 19/19/19 18/18/18 17/17/17 +f 20/20/20 19/19/19 17/17/17 +s 6 +f 21/21/21 22/22/22 23/23/23 +f 21/21/21 23/23/23 24/24/24 + + \ No newline at end of file diff --git a/data/jenga/jenga.png b/data/jenga/jenga.png new file mode 100644 index 000000000..1564309d9 Binary files /dev/null and b/data/jenga/jenga.png differ diff --git a/data/jenga/jenga.urdf b/data/jenga/jenga.urdf new file mode 100644 index 000000000..6ca727c78 --- /dev/null +++ b/data/jenga/jenga.urdf @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/data/l_finger_collision.stl b/data/l_finger_collision.stl new file mode 100644 index 000000000..b4e45652b Binary files /dev/null and b/data/l_finger_collision.stl differ diff --git a/data/plane.urdf b/data/plane.urdf index 57b746104..b2c2d7659 100644 --- a/data/plane.urdf +++ b/data/plane.urdf @@ -1,6 +1,6 @@ - + diff --git a/data/pr2_gripper.urdf b/data/pr2_gripper.urdf index 6f9a5b496..6ca8dc674 100644 --- a/data/pr2_gripper.urdf +++ b/data/pr2_gripper.urdf @@ -17,7 +17,7 @@ - + @@ -32,23 +32,23 @@ - - + + - + - - - + + + - + @@ -60,7 +60,8 @@ - + + @@ -72,10 +73,10 @@ - + - + @@ -90,22 +91,23 @@ - + + - + - - - - + + + + - + @@ -117,7 +119,8 @@ - + + @@ -127,12 +130,12 @@ - + - + - + diff --git a/data/samurai.urdf b/data/samurai.urdf index f15d47fb6..0a01099cd 100644 --- a/data/samurai.urdf +++ b/data/samurai.urdf @@ -1,9 +1,6 @@ - - - diff --git a/data/samurai_monastry.obj b/data/samurai_monastry.obj index 75b2a7833..435aa18da 100644 --- a/data/samurai_monastry.obj +++ b/data/samurai_monastry.obj @@ -331416,11 +331416,3 @@ f 30335 30336 f 1329 1330 f 77715 77717 f 20619 20622 -o Cube -v -0.500000 -0.200000 -1.000000 -v 1.000000 -0.000000 0.000000 -v -0.500000 0.200000 1.000000 -usemtl Material -s off -f 86188 86189 86190 -f 86189 86188 86190 diff --git a/data/sphere2.urdf b/data/sphere2.urdf index cf7618d93..f00423887 100644 --- a/data/sphere2.urdf +++ b/data/sphere2.urdf @@ -1,6 +1,11 @@ + + + + + diff --git a/data/teddy_vhacd.urdf b/data/teddy_vhacd.urdf new file mode 100644 index 000000000..7df9e7919 --- /dev/null +++ b/data/teddy_vhacd.urdf @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp index ecd1ed45d..ff499f0be 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp +++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp @@ -505,6 +505,8 @@ void OpenGLGuiHelper::autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWor void OpenGLGuiHelper::drawText3D( const char* txt, float posX, float posY, float posZ, float size) { + B3_PROFILE("OpenGLGuiHelper::drawText3D"); + btAssert(m_data->m_glApp); m_data->m_glApp->drawText3D(txt,posX,posY,posZ,size); } diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index b2e528677..320c0b6e6 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -14,7 +14,7 @@ subject to the following restrictions: #include "BulletUrdfImporter.h" #include "../../CommonInterfaces/CommonRenderInterface.h" - +#include"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h" #include "URDFImporterInterface.h" #include "btBulletCollisionCommon.h" #include "../ImportObjDemo/LoadMeshFromObj.h" @@ -28,6 +28,7 @@ subject to the following restrictions: #include "../ImportMeshUtility/b3ImportMeshUtility.h" +static btScalar gUrdfDefaultCollisionMargin = 0.001; #include #include @@ -295,26 +296,66 @@ void BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVect if (linkPtr) { UrdfLink* link = *linkPtr; + btMatrix3x3 linkInertiaBasis; + btScalar linkMass, principalInertiaX, principalInertiaY, principalInertiaZ; if (link->m_parentJoint==0 && m_data->m_urdfParser.getModel().m_overrideFixedBase) { - mass = 0.f; - localInertiaDiagonal.setValue(0,0,0); + linkMass = 0.f; + principalInertiaX = 0.f; + principalInertiaY = 0.f; + principalInertiaZ = 0.f; + linkInertiaBasis.setIdentity(); } else { - mass = link->m_inertia.m_mass; - localInertiaDiagonal.setValue(link->m_inertia.m_ixx,link->m_inertia.m_iyy, - link->m_inertia.m_izz); + linkMass = link->m_inertia.m_mass; + if (link->m_inertia.m_ixy == 0.0 && + link->m_inertia.m_ixz == 0.0 && + link->m_inertia.m_iyz == 0.0) + { + principalInertiaX = link->m_inertia.m_ixx; + principalInertiaY = link->m_inertia.m_iyy; + principalInertiaZ = link->m_inertia.m_izz; + linkInertiaBasis.setIdentity(); + } + else + { + principalInertiaX = link->m_inertia.m_ixx; + btMatrix3x3 inertiaTensor(link->m_inertia.m_ixx, link->m_inertia.m_ixy, link->m_inertia.m_ixz, + link->m_inertia.m_ixy, link->m_inertia.m_iyy, link->m_inertia.m_iyz, + link->m_inertia.m_ixz, link->m_inertia.m_iyz, link->m_inertia.m_izz); + btScalar threshold = 1.0e-6; + int numIterations = 30; + inertiaTensor.diagonalize(linkInertiaBasis, threshold, numIterations); + principalInertiaX = inertiaTensor[0][0]; + principalInertiaY = inertiaTensor[1][1]; + principalInertiaZ = inertiaTensor[2][2]; + } } - inertialFrame = link->m_inertia.m_linkLocalFrame; - + mass = linkMass; + if (principalInertiaX < 0 || + principalInertiaX > (principalInertiaY + principalInertiaZ) || + principalInertiaY < 0 || + principalInertiaY > (principalInertiaX + principalInertiaZ) || + principalInertiaZ < 0 || + principalInertiaZ > (principalInertiaX + principalInertiaY)) + { + b3Warning("Bad inertia tensor properties, setting inertia to zero for link: %s\n", link->m_name.c_str()); + principalInertiaX = 0.f; + principalInertiaY = 0.f; + principalInertiaZ = 0.f; + linkInertiaBasis.setIdentity(); + } + localInertiaDiagonal.setValue(principalInertiaX, principalInertiaY, principalInertiaZ); + inertialFrame.setOrigin(link->m_inertia.m_linkLocalFrame.getOrigin()); + inertialFrame.setBasis(link->m_inertia.m_linkLocalFrame.getBasis()*linkInertiaBasis); } else - { - mass = 1.f; - localInertiaDiagonal.setValue(1,1,1); - inertialFrame.setIdentity(); - } + { + mass = 1.f; + localInertiaDiagonal.setValue(1,1,1); + inertialFrame.setIdentity(); + } } bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const @@ -360,7 +401,45 @@ bool BulletURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWor return true; } +static btCollisionShape* createConvexHullFromShapes(std::vector& shapes) +{ + btCompoundShape* compound = new btCompoundShape(); + btTransform identity; + identity.setIdentity(); + for (int s = 0; s<(int)shapes.size(); s++) + { + btConvexHullShape* convexHull = new btConvexHullShape(); + tinyobj::shape_t& shape = shapes[s]; + int faceCount = shape.mesh.indices.size(); + + for (int f = 0; faddPoint(pt,false); + + pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0], + shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1], + shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]); + convexHull->addPoint(pt, false); + + pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0], + shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1], + shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]); + convexHull->addPoint(pt, false); + } + + convexHull->recalcLocalAabb(); + convexHull->optimizeConvexHull(); + compound->addChildShape(identity,convexHull); + } + + return compound; +} btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, const char* urdfPathPrefix) { @@ -386,7 +465,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co } btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); - cylZShape->setMargin(0.001); + cylZShape->setMargin(gUrdfDefaultCollisionMargin); cylZShape->initializePolyhedralFeatures(); //btConvexShape* cylZShape = new btConeShapeZ(cyl->radius,cyl->length);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); @@ -403,7 +482,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co btBoxShape* boxShape = new btBoxShape(extents*0.5f); //btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5); shape = boxShape; - shape ->setMargin(0.001); + shape ->setMargin(gUrdfDefaultCollisionMargin); break; } case URDF_GEOM_SPHERE: @@ -412,7 +491,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co btScalar radius = collision->m_geometry.m_sphereRadius; btSphereShape* sphereShape = new btSphereShape(radius); shape = sphereShape; - shape ->setMargin(0.001); + shape ->setMargin(gUrdfDefaultCollisionMargin); break; break; @@ -467,7 +546,18 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co { case FILE_OBJ: { - glmesh = LoadMeshFromObj(fullPath,collisionPathPrefix); + if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH) + { + glmesh = LoadMeshFromObj(fullPath, collisionPathPrefix); + } + else + { + std::vector shapes; + std::string err = tinyobj::LoadObj(shapes, fullPath, collisionPathPrefix); + //create a convex hull for each shape, and store it in a btCompoundShape + shape = createConvexHullFromShapes(shapes); + return shape; + } break; } case FILE_STL: @@ -600,7 +690,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co //cylZShape->initializePolyhedralFeatures(); //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.); //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents); - cylZShape->setMargin(0.001); + cylZShape->setMargin(gUrdfDefaultCollisionMargin); shape = cylZShape; } } else @@ -659,7 +749,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha } btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3)); - cylZShape->setMargin(0.001); + cylZShape->setMargin(gUrdfDefaultCollisionMargin); convexColShape = cylZShape; break; } @@ -671,7 +761,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha btBoxShape* boxShape = new btBoxShape(extents*0.5f); //btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5); convexColShape = boxShape; - convexColShape->setMargin(0.001); + convexColShape->setMargin(gUrdfDefaultCollisionMargin); break; } case URDF_GEOM_SPHERE: @@ -679,7 +769,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha btScalar radius = visual->m_geometry.m_sphereRadius; btSphereShape* sphereShape = new btSphereShape(radius); convexColShape = sphereShape; - convexColShape->setMargin(0.001); + convexColShape->setMargin(gUrdfDefaultCollisionMargin); break; break; @@ -1077,7 +1167,7 @@ btCollisionShape* BulletURDFImporter::getAllocatedCollisionShape(int index) btCompoundShape* compoundShape = new btCompoundShape(); m_data->m_allocatedCollisionShapes.push_back(compoundShape); - compoundShape->setMargin(0.001); + compoundShape->setMargin(gUrdfDefaultCollisionMargin); UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex); btAssert(linkPtr); if (linkPtr) diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 8833dd15b..5478b5b7d 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -162,8 +162,6 @@ void ConvertURDF2BulletInternal( int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex); btRigidBody* parentRigidBody = 0; - //std::string name = u2b.getLinkName(urdfLinkIndex); - //b3Printf("link name=%s urdf link index=%d\n",name.c_str(),urdfLinkIndex); //b3Printf("mb link index = %d\n",mbLinkIndex); btTransform parentLocalInertialFrame; @@ -322,6 +320,14 @@ void ConvertURDF2BulletInternal( cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointDamping = jointDamping; cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointFriction= jointFriction; creation.addLinkMapping(urdfLinkIndex,mbLinkIndex); + if (jointLowerLimit <= jointUpperLimit) + { + //std::string name = u2b.getLinkName(urdfLinkIndex); + //printf("create btMultiBodyJointLimitConstraint for revolute link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit, jointUpperLimit); + + btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit); + world1->addMultiBodyConstraint(con); + } } else { @@ -343,8 +349,14 @@ void ConvertURDF2BulletInternal( -offsetInB.getOrigin(), disableParentCollision); creation.addLinkMapping(urdfLinkIndex,mbLinkIndex); - btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody,mbLinkIndex,jointLowerLimit, jointUpperLimit); - world1->addMultiBodyConstraint(con); + if (jointLowerLimit <= jointUpperLimit) + { + //std::string name = u2b.getLinkName(urdfLinkIndex); + //printf("create btMultiBodyJointLimitConstraint for prismatic link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit,jointUpperLimit); + + btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit); + world1->addMultiBodyConstraint(con); + } //printf("joint lower limit=%d, upper limit = %f\n", jointLowerLimit, jointUpperLimit); } else @@ -386,8 +398,9 @@ void ConvertURDF2BulletInternal( //when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider col->setWorldTransform(tr); - - bool isDynamic = true; + + //base and fixed? -> static, otherwise flag as dynamic + bool isDynamic = (mbLinkIndex<0 && cache.m_bulletMultiBody->hasFixedBase())? false : true; short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index e1d9629a0..3abc77fa2 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -816,7 +816,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi bool UrdfParser::parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger) { joint.m_lowerLimit = 0.f; - joint.m_upperLimit = 0.f; + joint.m_upperLimit = -1.f; joint.m_effortLimit = 0.f; joint.m_velocityLimit = 0.f; joint.m_jointDamping = 0.f; diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 0ed995f33..9640f3666 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -138,7 +138,7 @@ struct UrdfJoint double m_jointFriction; UrdfJoint() :m_lowerLimit(0), - m_upperLimit(0), + m_upperLimit(-1), m_effortLimit(0), m_velocityLimit(0), m_jointDamping(0), diff --git a/examples/OpenGLWindow/GLPrimitiveRenderer.cpp b/examples/OpenGLWindow/GLPrimitiveRenderer.cpp index 60328514e..49cdbaed1 100644 --- a/examples/OpenGLWindow/GLPrimitiveRenderer.cpp +++ b/examples/OpenGLWindow/GLPrimitiveRenderer.cpp @@ -1,7 +1,7 @@ #ifndef NO_OPENGL3 #include "GLPrimitiveRenderer.h" #include "GLPrimInternalData.h" - +//#include "Bullet3Common/b3Logging.h" #include "LoadShader.h" #include @@ -205,7 +205,8 @@ void GLPrimitiveRenderer::drawRect(float x0, float y0, float x1, float y1, float void GLPrimitiveRenderer::drawTexturedRect3D(const PrimVertex& v0,const PrimVertex& v1,const PrimVertex& v2,const PrimVertex& v3,float viewMat[16],float projMat[16], bool useRGBA) { - + //B3_PROFILE("GLPrimitiveRenderer::drawTexturedRect3D"); + assert(glGetError()==GL_NO_ERROR); diff --git a/examples/OpenGLWindow/SimpleOpenGL3App.cpp b/examples/OpenGLWindow/SimpleOpenGL3App.cpp index 25d283119..a7254f889 100644 --- a/examples/OpenGLWindow/SimpleOpenGL3App.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL3App.cpp @@ -230,7 +230,7 @@ struct sth_stash* SimpleOpenGL3App::getFontStash() void SimpleOpenGL3App::drawText3D( const char* txt, float worldPosX, float worldPosY, float worldPosZ, float size1) { - + B3_PROFILE("SimpleOpenGL3App::drawText3D"); float viewMat[16]; float projMat[16]; CommonCameraInterface* cam = m_instancingRenderer->getActiveCamera(); @@ -273,6 +273,7 @@ void SimpleOpenGL3App::drawText3D( const char* txt, float worldPosX, float world bool measureOnly = false; float fontSize= 32;//64;//512;//128; + sth_draw_text(m_data->m_fontStash, m_data->m_droidRegular,fontSize,posX,posY, txt,&dx, this->m_instancingRenderer->getScreenWidth(),this->m_instancingRenderer->getScreenHeight(),measureOnly,m_window->getRetinaScale()); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 0639eea64..059eb56c6 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -28,6 +28,7 @@ #include "SharedMemoryCommands.h" btVector3 gLastPickPos(0, 0, 0); +bool gEnableRealTimeSimVR=false; struct UrdfLinkNameMapUtil { @@ -556,7 +557,7 @@ PhysicsServerCommandProcessor::PhysicsServerCommandProcessor() m_data = new PhysicsServerCommandProcessorInternalData(); createEmptyDynamicsWorld(); - m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.0; + m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.0001; } @@ -597,6 +598,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0)); m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.05; + } void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies() @@ -2838,7 +2840,7 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName) m_data->m_logPlayback = pb; } -btVector3 gVRGripperPos(0,0,0); +btVector3 gVRGripperPos(0,0,0.2); btQuaternion gVRGripperOrn(0,0,0,1); btScalar gVRGripperAnalog = 0; bool gVRGripperClosed = false; @@ -2850,10 +2852,11 @@ double gDtInSec = 0.f; double gSubStep = 0.f; void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) { - if (m_data->m_allowRealTimeSimulation && m_data->m_guiHelper) + if ((gEnableRealTimeSimVR || m_data->m_allowRealTimeSimulation) && m_data->m_guiHelper) { static btAlignedObjectArray gBufferServerToClient; - gBufferServerToClient.resize(32768); + gBufferServerToClient.resize(SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + if (!m_data->m_hasGround) @@ -2864,7 +2867,8 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - + loadUrdf("samurai.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + if (m_data->m_gripperRigidbodyFixed == 0) { int bodyId = 0; @@ -2874,11 +2878,11 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) InteralBodyData* parentBody = m_data->getHandle(bodyId); if (parentBody->m_multiBody) { - parentBody->m_multiBody->setHasSelfCollision(1); - btVector3 pivotInParent(0, 0, 0); + parentBody->m_multiBody->setHasSelfCollision(0); + btVector3 pivotInParent(0.2, 0, 0); btMatrix3x3 frameInParent; - frameInParent.setRotation(btQuaternion(0, 0, 0, 1)); - + //frameInParent.setRotation(btQuaternion(0, 0, 0, 1)); + frameInParent.setIdentity(); btVector3 pivotInChild(0, 0, 0); btMatrix3x3 frameInChild; frameInChild.setIdentity(); @@ -2890,62 +2894,110 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) m_data->m_gripperMultiBody->setJointPos(0, 0); m_data->m_gripperMultiBody->setJointPos(2, 0); } - m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(1.); + m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(10000); btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld; world->addMultiBodyConstraint(m_data->m_gripperRigidbodyFixed); } } - +#if 1 + for (int i = 0; i < 10; i++) + { + loadUrdf("cube.urdf", btVector3(3, -2, 0.5+i), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + } + loadUrdf("sphere2.urdf", btVector3(-2, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("sphere2.urdf", btVector3(-2, 0, 2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("sphere2.urdf", btVector3(-2, 0, 3), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + loadUrdf("r2d2.urdf", btVector3(2, -2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("samurai.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + loadUrdf("kuka_iiwa/model.urdf", btVector3(3, 0, 0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("cube_small.urdf", btVector3(0, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - loadUrdf("husky/husky.urdf", btVector3(1, 1, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + loadUrdf("cube_small.urdf", btVector3(0.3, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + + + // loadUrdf("cube_small.urdf", btVector3(0, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); +#endif +#if 0 + int jengaHeight = 10; + for (int j = 0; j < jengaHeight; j++) + { + for (int i = 0; i < 3; i++) + { + if (j & 1) + { + loadUrdf("jenga/jenga.urdf", btVector3(-0.5, 0.05*i, .03*0.5 + .03*j), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + } + else + { + btQuaternion orn(btVector3(0, 0, 1), SIMD_HALF_PI); + loadUrdf("jenga/jenga.urdf", btVector3(-0.5 -1 / 3.*0.15 + 0.05*i, +1 / 3.*0.15,0.03*0.5 + .03*j), orn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + } + } + } +#endif + + //loadUrdf("nao/nao.urdf", btVector3(2,5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + loadUrdf("husky/husky.urdf", btVector3(5, 2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); m_data->m_huskyId = bodyId; } + loadSdf("kiva_shelf/model.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true); + loadUrdf("teddy_vhacd.urdf", btVector3(1, 1, 2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + + m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10)); + } if (m_data->m_gripperRigidbodyFixed && m_data->m_gripperMultiBody) { m_data->m_gripperRigidbodyFixed->setFrameInB(btMatrix3x3(gVRGripperOrn)); m_data->m_gripperRigidbodyFixed->setPivotInB(gVRGripperPos); - if (m_data->m_gripperMultiBody->getNumLinks() > 2) + for (int i = 0; i < m_data->m_gripperMultiBody->getNumLinks(); i++) { - for (int i = 0; i < 2; i++) + if (supportsJointMotor(m_data->m_gripperMultiBody, i)) { - if (supportsJointMotor(m_data->m_gripperMultiBody, i * 2)) + btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)m_data->m_gripperMultiBody->getLink(i ).m_userPtr; + if (motor) { - btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)m_data->m_gripperMultiBody->getLink(i * 2).m_userPtr; - if (motor) + motor->setErp(0.2); + btScalar posTarget = 0.1 + (1 - btMin(btScalar(0.75),gVRGripperAnalog)*btScalar(1.5))*SIMD_HALF_PI*0.29; + btScalar maxPosTarget = 0.55; + + if (m_data->m_gripperMultiBody->getJointPos(i) < 0) { - motor->setErp(0.01); - - motor->setPositionTarget(0.1+(1-gVRGripperAnalog)*SIMD_HALF_PI*0.5, 1); - - - motor->setVelocityTarget(0, 0.1); - btScalar maxImp = 1550.*m_data->m_physicsDeltaTime; - motor->setMaxAppliedImpulse(maxImp); + m_data->m_gripperMultiBody->setJointPos(i,0); } + if (m_data->m_gripperMultiBody->getJointPos(i) > maxPosTarget) + { + m_data->m_gripperMultiBody->setJointPos(i, maxPosTarget); + } + + motor->setPositionTarget(posTarget, 1); + motor->setVelocityTarget(0, 0.5); + btScalar maxImp = 1*m_data->m_physicsDeltaTime; + motor->setMaxAppliedImpulse(maxImp); } } } } - int maxSteps = 3; - - int numSteps = m_data->m_dynamicsWorld->stepSimulation(dtInSec,maxSteps,m_data->m_physicsDeltaTime); + int maxSteps = m_data->m_numSimulationSubSteps+3; + if (m_data->m_numSimulationSubSteps) + { + gSubStep = m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps; + } + else + { + gSubStep = m_data->m_physicsDeltaTime; + } + + int numSteps = m_data->m_dynamicsWorld->stepSimulation(dtInSec,maxSteps, gSubStep); gDroppedSimulationSteps += numSteps > maxSteps ? numSteps - maxSteps : 0; if (numSteps) { gNumSteps = numSteps; gDtInSec = dtInSec; - gSubStep = m_data->m_physicsDeltaTime; } } } diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 837bb5c21..2dec1401c 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -19,6 +19,10 @@ extern btVector3 gLastPickPos; btVector3 gVRTeleportPos(0,0,0); btQuaternion gVRTeleportOrn(0, 0, 0,1); +extern btVector3 gVRGripperPos; +extern btQuaternion gVRGripperOrn; +extern btScalar gVRGripperAnalog; +extern bool gEnableRealTimeSimVR; extern bool gVRGripperClosed; @@ -893,6 +897,8 @@ extern double gSubStep; void PhysicsServerExample::renderScene() { + B3_PROFILE("PhysicsServerExample::RenderScene"); + ///debug rendering //m_args[0].m_cs->lock(); @@ -934,6 +940,8 @@ void PhysicsServerExample::renderScene() if (gDebugRenderToggle) if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera()) { + gEnableRealTimeSimVR = true; + B3_PROFILE("Draw Debug HUD"); //some little experiment to add text/HUD to a VR camera (HTC Vive/Oculus Rift) static int frameCount=0; @@ -1154,16 +1162,12 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt } } -extern btVector3 gVRGripperPos; -extern btQuaternion gVRGripperOrn; -extern btScalar gVRGripperAnalog; - void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4], float analogAxis) { - + gEnableRealTimeSimVR = true; if (controllerId <= 0 || controllerId >= MAX_VR_CONTROLLERS) { diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp index 32941de76..485ee71ca 100644 --- a/examples/StandaloneMain/hellovr_opengl_main.cpp +++ b/examples/StandaloneMain/hellovr_opengl_main.cpp @@ -1,4 +1,5 @@ #ifdef BT_ENABLE_VR +//#define BT_USE_CUSTOM_PROFILER //========= Copyright Valve Corporation ============// #include "../OpenGLWindow/SimpleOpenGL3App.h" @@ -701,11 +702,12 @@ bool CMainApplication::HandleInput() { glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); ///todo(erwincoumans) can't use reguar debug drawer, because physics/graphics are not in sync + //so it can (and likely will) cause crashes //add a special debug drawer that deals with this - //gDebugDrawFlags = btIDebugDraw::DBG_DrawWireframe;// :DBG_DrawContactPoints + //gDebugDrawFlags = btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints+ //btIDebugDraw::DBG_DrawConstraintLimits+ //btIDebugDraw::DBG_DrawConstraints - ; + //; } sExample->vrControllerButtonCallback(unDevice, button, 1, pos, orn); @@ -751,7 +753,7 @@ bool CMainApplication::HandleInput() } } - m_rbShowTrackedDevice[ unDevice ] = state.ulButtonPressed == 0; +// m_rbShowTrackedDevice[ unDevice ] = state.ulButtonPressed == 0; } sPrevStates[unDevice] = state; } @@ -769,6 +771,8 @@ void CMainApplication::RunMainLoop() while ( !bQuit && !m_app->m_window->requestedExit()) { + B3_PROFILE("main"); + bQuit = HandleInput(); RenderFrame(); @@ -812,9 +816,15 @@ void CMainApplication::RenderFrame() // for now as fast as possible if ( m_pHMD ) { - DrawControllers(); + { + B3_PROFILE("DrawControllers"); + DrawControllers(); + } RenderStereoTargets(); - RenderDistortion(); + { + B3_PROFILE("RenderDistortion"); + RenderDistortion(); + } vr::Texture_t leftEyeTexture = {(void*)leftEyeDesc.m_nResolveTextureId, vr::API_OpenGL, vr::ColorSpace_Gamma }; vr::VRCompositor()->Submit(vr::Eye_Left, &leftEyeTexture ); @@ -824,6 +834,7 @@ void CMainApplication::RenderFrame() if ( m_bVblank && m_bGlFinishHack ) { + B3_PROFILE("bGlFinishHack"); //$ HACKHACK. From gpuview profiling, it looks like there is a bug where two renders and a present // happen right before and after the vsync causing all kinds of jittering issues. This glFinish() // appears to clear that up. Temporary fix while I try to get nvidia to investigate this problem. @@ -833,6 +844,7 @@ void CMainApplication::RenderFrame() // SwapWindow { + B3_PROFILE("m_app->swapBuffer"); m_app->swapBuffer(); //SDL_GL_SwapWindow( m_pWindow ); @@ -840,6 +852,7 @@ void CMainApplication::RenderFrame() // Clear { + B3_PROFILE("glClearColor"); // We want to make sure the glFinish waits for the entire present to complete, not just the submission // of the command. So, we do a clear here right here so the glFinish will wait fully for the swap. glClearColor( 0, 0, 0, 1 ); @@ -849,6 +862,8 @@ void CMainApplication::RenderFrame() // Flush and wait for swap. if ( m_bVblank ) { + B3_PROFILE("glFlushglFinish"); + glFlush(); glFinish(); } @@ -856,13 +871,18 @@ void CMainApplication::RenderFrame() // Spew out the controller and pose count whenever they change. if ( m_iTrackedControllerCount != m_iTrackedControllerCount_Last || m_iValidPoseCount != m_iValidPoseCount_Last ) { + B3_PROFILE("debug pose"); + m_iValidPoseCount_Last = m_iValidPoseCount; m_iTrackedControllerCount_Last = m_iTrackedControllerCount; b3Printf( "PoseCount:%d(%s) Controllers:%d\n", m_iValidPoseCount, m_strPoseClasses.c_str(), m_iTrackedControllerCount ); } - UpdateHMDMatrixPose(); + { + B3_PROFILE("UpdateHMDMatrixPose"); + UpdateHMDMatrixPose(); + } } @@ -1565,6 +1585,8 @@ void CMainApplication::SetupDistortion() //----------------------------------------------------------------------------- void CMainApplication::RenderStereoTargets() { + B3_PROFILE("CMainApplication::RenderStereoTargets"); + sExample->stepSimulation(1./60.); glClearColor( 0.15f, 0.15f, 0.18f, 1.0f ); // nice background color, but not black @@ -1641,7 +1663,7 @@ void CMainApplication::RenderStereoTargets() { sExample->physicsDebugDraw(gDebugDrawFlags); } - + else { sExample->renderScene(); } @@ -1692,7 +1714,7 @@ void CMainApplication::RenderStereoTargets() { sExample->physicsDebugDraw(gDebugDrawFlags); } - + else { sExample->renderScene(); } @@ -1720,6 +1742,8 @@ void CMainApplication::RenderStereoTargets() //----------------------------------------------------------------------------- void CMainApplication::RenderScene( vr::Hmd_Eye nEye ) { + B3_PROFILE("RenderScene"); + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glEnable(GL_DEPTH_TEST); @@ -1866,38 +1890,47 @@ Matrix4 CMainApplication::GetCurrentViewProjectionMatrix( vr::Hmd_Eye nEye ) //----------------------------------------------------------------------------- void CMainApplication::UpdateHMDMatrixPose() { - if ( !m_pHMD ) + if (!m_pHMD) return; - - vr::VRCompositor()->WaitGetPoses(m_rTrackedDevicePose, vr::k_unMaxTrackedDeviceCount, NULL, 0 ); + { + B3_PROFILE("WaitGetPoses"); + vr::VRCompositor()->WaitGetPoses(m_rTrackedDevicePose, vr::k_unMaxTrackedDeviceCount, NULL, 0); + } m_iValidPoseCount = 0; m_strPoseClasses = ""; - for ( int nDevice = 0; nDevice < vr::k_unMaxTrackedDeviceCount; ++nDevice ) { - if ( m_rTrackedDevicePose[nDevice].bPoseIsValid ) + B3_PROFILE("for loop"); + + for (int nDevice = 0; nDevice < vr::k_unMaxTrackedDeviceCount; ++nDevice) { - m_iValidPoseCount++; - m_rmat4DevicePose[nDevice] = ConvertSteamVRMatrixToMatrix4( m_rTrackedDevicePose[nDevice].mDeviceToAbsoluteTracking ); - if (m_rDevClassChar[nDevice]==0) + if (m_rTrackedDevicePose[nDevice].bPoseIsValid) { - switch (m_pHMD->GetTrackedDeviceClass(nDevice)) + m_iValidPoseCount++; + m_rmat4DevicePose[nDevice] = ConvertSteamVRMatrixToMatrix4(m_rTrackedDevicePose[nDevice].mDeviceToAbsoluteTracking); + if (m_rDevClassChar[nDevice] == 0) { - case vr::TrackedDeviceClass_Controller: m_rDevClassChar[nDevice] = 'C'; break; - case vr::TrackedDeviceClass_HMD: m_rDevClassChar[nDevice] = 'H'; break; - case vr::TrackedDeviceClass_Invalid: m_rDevClassChar[nDevice] = 'I'; break; - case vr::TrackedDeviceClass_Other: m_rDevClassChar[nDevice] = 'O'; break; - case vr::TrackedDeviceClass_TrackingReference: m_rDevClassChar[nDevice] = 'T'; break; - default: m_rDevClassChar[nDevice] = '?'; break; + switch (m_pHMD->GetTrackedDeviceClass(nDevice)) + { + case vr::TrackedDeviceClass_Controller: m_rDevClassChar[nDevice] = 'C'; break; + case vr::TrackedDeviceClass_HMD: m_rDevClassChar[nDevice] = 'H'; break; + case vr::TrackedDeviceClass_Invalid: m_rDevClassChar[nDevice] = 'I'; break; + case vr::TrackedDeviceClass_Other: m_rDevClassChar[nDevice] = 'O'; break; + case vr::TrackedDeviceClass_TrackingReference: m_rDevClassChar[nDevice] = 'T'; break; + default: m_rDevClassChar[nDevice] = '?'; break; + } } + m_strPoseClasses += m_rDevClassChar[nDevice]; } - m_strPoseClasses += m_rDevClassChar[nDevice]; } } - - if ( m_rTrackedDevicePose[vr::k_unTrackedDeviceIndex_Hmd].bPoseIsValid ) { - m_mat4HMDPose = m_rmat4DevicePose[vr::k_unTrackedDeviceIndex_Hmd].invert(); + B3_PROFILE("m_mat4HMDPose invert"); + + if (m_rTrackedDevicePose[vr::k_unTrackedDeviceIndex_Hmd].bPoseIsValid) + { + m_mat4HMDPose = m_rmat4DevicePose[vr::k_unTrackedDeviceIndex_Hmd].invert(); + } } } @@ -2145,6 +2178,11 @@ void CGLRenderModel::Draw() //----------------------------------------------------------------------------- int main(int argc, char *argv[]) { +#ifdef BT_USE_CUSTOM_PROFILER + //b3SetCustomEnterProfileZoneFunc(...); + //b3SetCustomLeaveProfileZoneFunc(...); +#endif + CMainApplication *pMainApplication = new CMainApplication( argc, argv ); if (!pMainApplication->BInit()) @@ -2157,6 +2195,10 @@ int main(int argc, char *argv[]) pMainApplication->Shutdown(); +#ifdef BT_USE_CUSTOM_PROFILER +//... +#endif + return 0; } #endif //BT_ENABLE_VR \ No newline at end of file diff --git a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp index ad69fcbd7..55ebf06f1 100644 --- a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp +++ b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp @@ -372,10 +372,10 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro return userData; } //#include - +#include "LinearMath/btQuickprof.h" void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher) { - + BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs"); int i; // printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size()); diff --git a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index 3b6913c0e..d41e98c32 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -16,7 +16,7 @@ subject to the following restrictions: #include "btCollisionDispatcher.h" - +#include "LinearMath/btQuickprof.h" #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" @@ -227,6 +227,8 @@ public: virtual bool processOverlap(btBroadphasePair& pair) { + BT_PROFILE("btCollisionDispatcher::processOverlap"); + (*m_dispatcher->getNearCallback())(pair,*m_dispatcher,m_dispatchInfo); return false; @@ -249,7 +251,6 @@ void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pa - //by default, Bullet will use this near callback void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo) { diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp index 1d64d84b8..ab2632ee3 100644 --- a/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp @@ -15,6 +15,7 @@ subject to the following restrictions: */ #include "btCompoundCompoundCollisionAlgorithm.h" +#include "LinearMath/btQuickprof.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionShapes/btCompoundShape.h" #include "BulletCollision/BroadphaseCollision/btDbvt.h" @@ -124,6 +125,7 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide void Process(const btDbvtNode* leaf0,const btDbvtNode* leaf1) { + BT_PROFILE("btCompoundCompoundLeafCallback::Process"); m_numOverlapPairs++; diff --git a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp index 912a52855..322b1288d 100644 --- a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -15,6 +15,7 @@ subject to the following restrictions: #include "btConvexConcaveCollisionAlgorithm.h" +#include "LinearMath/btQuickprof.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionShapes/btMultiSphereShape.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" @@ -79,6 +80,7 @@ void btConvexTriangleCallback::clearCache() void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex) { + BT_PROFILE("btConvexTriangleCallback::processTriangle"); if (!TestTriangleAgainstAabb2(triangle, m_aabbMin, m_aabbMax)) { @@ -184,7 +186,7 @@ void btConvexConcaveCollisionAlgorithm::clearCache() void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { - + BT_PROFILE("btConvexConcaveCollisionAlgorithm::processCollision"); const btCollisionObjectWrapper* convexBodyWrap = m_isSwapped ? body1Wrap : body0Wrap; const btCollisionObjectWrapper* triBodyWrap = m_isSwapped ? body0Wrap : body1Wrap; @@ -265,6 +267,7 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) { + BT_PROFILE("processTriangle"); (void)partId; (void)triangleIndex; //do a swept sphere for now diff --git a/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp b/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp index ace4cfa26..0940da1a4 100644 --- a/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp +++ b/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp @@ -245,16 +245,18 @@ void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co btStridingMeshInterface* m_meshInterface; btTriangleCallback* m_callback; btVector3 m_triangle[3]; - + int m_numOverlap; MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface) :m_meshInterface(meshInterface), - m_callback(callback) + m_callback(callback), + m_numOverlap(0) { } virtual void processNode(int nodeSubPart, int nodeTriangleIndex) { + m_numOverlap++; const unsigned char *vertexbase; int numverts; PHY_ScalarType type; @@ -321,8 +323,7 @@ void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface); m_bvh->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax); - - + #endif//DISABLE_BVH diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp index 603ad167d..a23ee3d88 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -39,7 +39,7 @@ subject to the following restrictions: //temp globals, to improve GJK/EPA/penetration calculations int gNumDeepPenetrationChecks = 0; int gNumGjkChecks = 0; - +btScalar gGjkEpaPenetrationTolerance = 0.001; btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver) :m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)), @@ -304,7 +304,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu } bool catchDegeneratePenetrationCase = - (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01)); + (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < gGjkEpaPenetrationTolerance)); //if (checkPenetration && !isValid) if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))