diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 4302ca347..9930ac994 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -1,6 +1,9 @@ + #include #include "LinearMath/btTransform.h" +#include "LinearMath/btThreads.h" #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" + #include "BulletCollision/CollisionShapes/btCompoundShape.h" #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" @@ -28,10 +31,13 @@ static btVector4 colors[4] = static btVector4 selectColor2() { + static btSpinMutex sMutex; + sMutex.lock(); static int curColor = 0; btVector4 color = colors[curColor]; curColor++; curColor &= 3; + sMutex.unlock(); return color; } diff --git a/src/BulletCollision/CollisionShapes/btShapeHull.cpp b/src/BulletCollision/CollisionShapes/btShapeHull.cpp index 8f44e31f1..a2c490faf 100644 --- a/src/BulletCollision/CollisionShapes/btShapeHull.cpp +++ b/src/BulletCollision/CollisionShapes/btShapeHull.cpp @@ -37,21 +37,8 @@ btShapeHull::~btShapeHull() bool btShapeHull::buildHull(btScalar /*margin*/, int highres) { + int numSampleDirections = highres ? NUM_UNITSPHERE_POINTS_HIGHRES : NUM_UNITSPHERE_POINTS; - { - int numPDA = m_shape->getNumPreferredPenetrationDirections(); - if (numPDA) - { - for (int i = 0; i < numPDA; i++) - { - btVector3 norm; - m_shape->getPreferredPenetrationDirection(i, norm); - getUnitSpherePoints(highres)[numSampleDirections] = norm; - numSampleDirections++; - } - } - } - btVector3 supportPoints[NUM_UNITSPHERE_POINTS_HIGHRES + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2]; int i; for (i = 0; i < numSampleDirections; i++) @@ -59,6 +46,17 @@ bool btShapeHull::buildHull(btScalar /*margin*/, int highres) supportPoints[i] = m_shape->localGetSupportingVertex(getUnitSpherePoints(highres)[i]); } + int numPDA = m_shape->getNumPreferredPenetrationDirections(); + if (numPDA) + { + for (int s = 0; s < numPDA; s++) + { + btVector3 norm; + m_shape->getPreferredPenetrationDirection(s, norm); + supportPoints[i++] = m_shape->localGetSupportingVertex(norm); + numSampleDirections++; + } + } HullDesc hd; hd.mFlags = QF_TRIANGLES; hd.mVcount = static_cast(numSampleDirections);