allow to enable/disable implicit cylinder conversion through an API

p.loadURDF("r2d2.urdf", flags=p.URDF_USE_IMPLICIT_CYLINDER)
allow to enable/disable deterministicOverlappingPairs through an API
p.setPhysicsEngineParameter(deterministicOverlappingPairs = False)
This commit is contained in:
erwincoumans
2018-01-09 10:10:36 -08:00
parent bf6d805228
commit e138e85bca
11 changed files with 85 additions and 47 deletions

View File

@@ -25,7 +25,7 @@ subject to the following restrictions:
#include "Bullet3Common/b3FileUtils.h" #include "Bullet3Common/b3FileUtils.h"
#include <string> #include <string>
#include "../../Utils/b3ResourcePath.h" #include "../../Utils/b3ResourcePath.h"
#include "URDF2Bullet.h"//for flags
#include "../ImportMeshUtility/b3ImportMeshUtility.h" #include "../ImportMeshUtility/b3ImportMeshUtility.h"
static btScalar gUrdfDefaultCollisionMargin = 0.001; static btScalar gUrdfDefaultCollisionMargin = 0.001;
@@ -53,6 +53,7 @@ ATTRIBUTE_ALIGNED16(struct) BulletURDFInternalData
LinkVisualShapesConverter* m_customVisualShapesConverter; LinkVisualShapesConverter* m_customVisualShapesConverter;
bool m_enableTinyRenderer; bool m_enableTinyRenderer;
int m_flags;
void setSourceFile(const std::string& relativeFileName, const std::string& prefix) void setSourceFile(const std::string& relativeFileName, const std::string& prefix)
{ {
@@ -66,6 +67,7 @@ ATTRIBUTE_ALIGNED16(struct) BulletURDFInternalData
{ {
m_enableTinyRenderer = true; m_enableTinyRenderer = true;
m_pathPrefix[0] = 0; m_pathPrefix[0] = 0;
m_flags=0;
} }
void setGlobalScaling(btScalar scaling) void setGlobalScaling(btScalar scaling)
@@ -80,10 +82,11 @@ void BulletURDFImporter::printTree()
// btAssert(0); // btAssert(0);
} }
BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter, double globalScaling) BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter, double globalScaling, int flags)
{ {
m_data = new BulletURDFInternalData; m_data = new BulletURDFInternalData;
m_data->setGlobalScaling(globalScaling); m_data->setGlobalScaling(globalScaling);
m_data->m_flags = flags;
m_data->m_guiHelper = helper; m_data->m_guiHelper = helper;
m_data->m_customVisualShapesConverter = customConverter; m_data->m_customVisualShapesConverter = customConverter;
@@ -612,16 +615,22 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
{ {
btScalar cylRadius = collision->m_geometry.m_capsuleRadius; btScalar cylRadius = collision->m_geometry.m_capsuleRadius;
btScalar cylHalfLength = 0.5*collision->m_geometry.m_capsuleHeight; btScalar cylHalfLength = 0.5*collision->m_geometry.m_capsuleHeight;
#if 0 if (m_data->m_flags & CUF_USE_IMPLICIT_CYLINDER)
{
btVector3 halfExtents(cylRadius,cylRadius, cylHalfLength);
btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
shape = cylZShape;
} else
{
btAlignedObjectArray<btVector3> vertices; btAlignedObjectArray<btVector3> vertices;
//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
int numSteps = 32; int numSteps = 32;
for (int i=0;i<numSteps;i++) for (int i=0;i<numSteps;i++)
{ {
btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i)/numSteps)),cylRadius*btCos(SIMD_2_PI*(float(i)/numSteps)),cylLength/2.); btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i)/numSteps)),cylRadius*btCos(SIMD_2_PI*(float(i)/numSteps)),cylHalfLength);
vertices.push_back(vert); vertices.push_back(vert);
vert[2] = -cylLength/2.; vert[2] = -cylHalfLength;
vertices.push_back(vert); vertices.push_back(vert);
} }
@@ -630,14 +639,9 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
cylZShape->recalcLocalAabb(); cylZShape->recalcLocalAabb();
cylZShape->initializePolyhedralFeatures(); cylZShape->initializePolyhedralFeatures();
cylZShape->optimizeConvexHull(); cylZShape->optimizeConvexHull();
//btConvexShape* cylZShape = new btConeShapeZ(cylRadius,cylLength);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
#else
btVector3 halfExtents(cylRadius,cylRadius, cylHalfLength);
btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
#endif
shape = cylZShape; shape = cylZShape;
}
break; break;
} }
case URDF_GEOM_BOX: case URDF_GEOM_BOX:

View File

@@ -23,7 +23,7 @@ class BulletURDFImporter : public URDFImporterInterface
public: public:
BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter, double globalScaling=1); BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter, double globalScaling, int flags);
virtual ~BulletURDFImporter(); virtual ~BulletURDFImporter();

View File

@@ -13,7 +13,7 @@ class URDFImporterInterface;
class MultiBodyCreationInterface; class MultiBodyCreationInterface;
//manually sync with eURDF_Flags in SharedMemoryPublic.h!
enum ConvertURDFFlags { enum ConvertURDFFlags {
CUF_USE_SDF = 1, CUF_USE_SDF = 1,
// Use inertia values in URDF instead of recomputing them from collision shape. // Use inertia values in URDF instead of recomputing them from collision shape.
@@ -23,6 +23,7 @@ enum ConvertURDFFlags {
CUF_USE_SELF_COLLISION_EXCLUDE_PARENT=16, CUF_USE_SELF_COLLISION_EXCLUDE_PARENT=16,
CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS=32, CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS=32,
CUF_RESERVED=64, CUF_RESERVED=64,
CUF_USE_IMPLICIT_CYLINDER=128,
}; };
void ConvertURDF2Bullet(const URDFImporterInterface& u2b, void ConvertURDF2Bullet(const URDFImporterInterface& u2b,

View File

@@ -557,6 +557,15 @@ B3_SHARED_API int b3PhysicsParamSetEnableConeFriction(b3SharedMemoryCommandHandl
return 0; return 0;
} }
B3_SHARED_API int b3PhysicsParameterSetDeterministicOverlappingPairs(b3SharedMemoryCommandHandle commandHandle, int deterministicOverlappingPairs)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_deterministicOverlappingPairs = deterministicOverlappingPairs;
command->m_updateFlags |= SIM_PARAM_UPDATE_DETERMINISTIC_OVERLAPPING_PAIRS;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations) B3_SHARED_API int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations)
{ {

View File

@@ -291,6 +291,8 @@ B3_SHARED_API int b3PhysicsParamSetMaxNumCommandsPer1ms(b3SharedMemoryCommandHan
B3_SHARED_API int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle commandHandle, int enableFileCaching); B3_SHARED_API int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle commandHandle, int enableFileCaching);
B3_SHARED_API int b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCommandHandle commandHandle, double restitutionVelocityThreshold); B3_SHARED_API int b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCommandHandle commandHandle, double restitutionVelocityThreshold);
B3_SHARED_API int b3PhysicsParamSetEnableConeFriction(b3SharedMemoryCommandHandle commandHandle, int enableConeFriction); B3_SHARED_API int b3PhysicsParamSetEnableConeFriction(b3SharedMemoryCommandHandle commandHandle, int enableConeFriction);
B3_SHARED_API int b3PhysicsParameterSetDeterministicOverlappingPairs(b3SharedMemoryCommandHandle commandHandle, int deterministicOverlappingPairs);

View File

@@ -2727,7 +2727,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
m_data->m_sdfRecentLoadedBodies.clear(); m_data->m_sdfRecentLoadedBodies.clear();
BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter, globalScaling); BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter, globalScaling, flags);
u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer); u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer);
bool forceFixedBase = false; bool forceFixedBase = false;
@@ -2759,7 +2759,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter, globalScaling); BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter, globalScaling, flags);
u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer); u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer);
bool loadOk = u2b.loadURDF(fileName, useFixedBase); bool loadOk = u2b.loadURDF(fileName, useFixedBase);
@@ -3770,12 +3770,12 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
btAlignedObjectArray<btVector3> convertedVerts; btAlignedObjectArray<btVector3> convertedVerts;
convertedVerts.reserve(glmesh->m_numvertices); convertedVerts.reserve(glmesh->m_numvertices);
for (int i = 0; i < glmesh->m_numvertices; i++) for (int v = 0; v < glmesh->m_numvertices; v++)
{ {
convertedVerts.push_back(btVector3( convertedVerts.push_back(btVector3(
glmesh->m_vertices->at(i).xyzw[0] * meshScale[0], glmesh->m_vertices->at(v).xyzw[0] * meshScale[0],
glmesh->m_vertices->at(i).xyzw[1] * meshScale[1], glmesh->m_vertices->at(v).xyzw[1] * meshScale[1],
glmesh->m_vertices->at(i).xyzw[2] * meshScale[2])); glmesh->m_vertices->at(v).xyzw[2] * meshScale[2]));
} }
if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags&GEOM_FORCE_CONCAVE_TRIMESH) if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags&GEOM_FORCE_CONCAVE_TRIMESH)
@@ -3868,8 +3868,8 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct
bool hasStatus = true; bool hasStatus = true;
serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_FAILED; serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_FAILED;
double globalScaling = 1.f; double globalScaling = 1.f;
int flags=0;
BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter, globalScaling); BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter, globalScaling, flags);
u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer); u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer);
btTransform localInertiaFrame; btTransform localInertiaFrame;
localInertiaFrame.setIdentity(); localInertiaFrame.setIdentity();
@@ -6502,6 +6502,10 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
m_data->m_dynamicsWorld->getSolverInfo().m_solverMode |=SOLVER_DISABLE_IMPLICIT_CONE_FRICTION; m_data->m_dynamicsWorld->getSolverInfo().m_solverMode |=SOLVER_DISABLE_IMPLICIT_CONE_FRICTION;
} }
} }
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DETERMINISTIC_OVERLAPPING_PAIRS)
{
m_data->m_dynamicsWorld->getDispatchInfo().m_deterministicOverlappingPairs = clientCmd.m_physSimParamArgs.m_deterministicOverlappingPairs;
}
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DELTA_TIME) if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DELTA_TIME)
{ {
m_data->m_physicsDeltaTime = clientCmd.m_physSimParamArgs.m_deltaTime; m_data->m_physicsDeltaTime = clientCmd.m_physSimParamArgs.m_deltaTime;

View File

@@ -437,6 +437,7 @@ enum EnumSimParamUpdateFlags
SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD = 8192, SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD = 8192,
SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP=16384, SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP=16384,
SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP = 32768, SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP = 32768,
SIM_PARAM_UPDATE_DETERMINISTIC_OVERLAPPING_PAIRS = 65536,
}; };
enum EnumLoadSoftBodyUpdateFlags enum EnumLoadSoftBodyUpdateFlags

View File

@@ -670,7 +670,7 @@ enum eURDF_Flags
URDF_USE_SELF_COLLISION_EXCLUDE_PARENT=16, URDF_USE_SELF_COLLISION_EXCLUDE_PARENT=16,
URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS=32, URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS=32,
URDF_RESERVED=64, URDF_RESERVED=64,
URDF_USE_IMPLICIT_CYLINDER =128,
}; };
enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes
@@ -733,6 +733,7 @@ struct b3PhysicsSimulationParameters
double m_defaultNonContactERP; double m_defaultNonContactERP;
double m_frictionERP; double m_frictionERP;
int m_enableConeFriction; int m_enableConeFriction;
int m_deterministicOverlappingPairs;
}; };

View File

@@ -1068,12 +1068,13 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
double frictionERP = -1; double frictionERP = -1;
int enableConeFriction = -1; int enableConeFriction = -1;
b3PhysicsClientHandle sm = 0; b3PhysicsClientHandle sm = 0;
int deterministicOverlappingPairs = -1;
int physicsClientId = 0; int physicsClientId = 0;
static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "physicsClientId", NULL}; static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
&collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &physicsClientId)) &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &physicsClientId))
{ {
return NULL; return NULL;
} }
@@ -1149,6 +1150,10 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
{ {
b3PhysicsParamSetEnableConeFriction(command, enableConeFriction); b3PhysicsParamSetEnableConeFriction(command, enableConeFriction);
} }
if (deterministicOverlappingPairs>=0)
{
b3PhysicsParameterSetDeterministicOverlappingPairs(command,deterministicOverlappingPairs);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
} }
@@ -8811,6 +8816,8 @@ initpybullet(void)
PyModule_AddIntConstant(m, "IK_HAS_JOINT_DAMPING", IK_HAS_JOINT_DAMPING); PyModule_AddIntConstant(m, "IK_HAS_JOINT_DAMPING", IK_HAS_JOINT_DAMPING);
PyModule_AddIntConstant(m, "URDF_USE_INERTIA_FROM_FILE", URDF_USE_INERTIA_FROM_FILE); PyModule_AddIntConstant(m, "URDF_USE_INERTIA_FROM_FILE", URDF_USE_INERTIA_FROM_FILE);
PyModule_AddIntConstant(m, "URDF_USE_IMPLICIT_CYLINDER", URDF_USE_IMPLICIT_CYLINDER);
PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION", URDF_USE_SELF_COLLISION); PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION", URDF_USE_SELF_COLLISION);
PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_PARENT", URDF_USE_SELF_COLLISION_EXCLUDE_PARENT); PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_PARENT", URDF_USE_SELF_COLLISION_EXCLUDE_PARENT);
PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS", URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS); PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS", URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS);
@@ -8861,6 +8868,7 @@ initpybullet(void)
PyModule_AddIntConstant(m, "STATE_LOG_JOINT_USER_TORQUES", STATE_LOG_JOINT_USER_TORQUES); PyModule_AddIntConstant(m, "STATE_LOG_JOINT_USER_TORQUES", STATE_LOG_JOINT_USER_TORQUES);
PyModule_AddIntConstant(m, "STATE_LOG_JOINT_TORQUES", STATE_LOG_JOINT_USER_TORQUES+STATE_LOG_JOINT_MOTOR_TORQUES); PyModule_AddIntConstant(m, "STATE_LOG_JOINT_TORQUES", STATE_LOG_JOINT_USER_TORQUES+STATE_LOG_JOINT_MOTOR_TORQUES);
SpamError = PyErr_NewException("pybullet.error", NULL, NULL); SpamError = PyErr_NewException("pybullet.error", NULL, NULL);
Py_INCREF(SpamError); Py_INCREF(SpamError);
PyModule_AddObject(m, "error", SpamError); PyModule_AddObject(m, "error", SpamError);

View File

@@ -46,7 +46,8 @@ struct btDispatcherInfo
m_useEpa(true), m_useEpa(true),
m_allowedCcdPenetration(btScalar(0.04)), m_allowedCcdPenetration(btScalar(0.04)),
m_useConvexConservativeDistanceUtil(false), m_useConvexConservativeDistanceUtil(false),
m_convexConservativeDistanceThreshold(0.0f) m_convexConservativeDistanceThreshold(0.0f),
m_deterministicOverlappingPairs(true)
{ {
} }
@@ -62,6 +63,7 @@ struct btDispatcherInfo
btScalar m_allowedCcdPenetration; btScalar m_allowedCcdPenetration;
bool m_useConvexConservativeDistanceUtil; bool m_useConvexConservativeDistanceUtil;
btScalar m_convexConservativeDistanceThreshold; btScalar m_convexConservativeDistanceThreshold;
bool m_deterministicOverlappingPairs;
}; };
enum ebtDispatcherQueryType enum ebtDispatcherQueryType

View File

@@ -334,8 +334,11 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio
for (i=0;i<maxNumManifolds ;i++) for (i=0;i<maxNumManifolds ;i++)
{ {
btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i); btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs)
{
if (manifold->getNumContacts() == 0) if (manifold->getNumContacts() == 0)
continue; continue;
}
const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0()); const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1()); const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1());
@@ -397,12 +400,15 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
//tried a radix sort, but quicksort/heapsort seems still faster //tried a radix sort, but quicksort/heapsort seems still faster
//@todo rewrite island management //@todo rewrite island management
//m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
//btPersistentManifoldSortPredicateDeterministic sorts contact manifolds based on islandid, //btPersistentManifoldSortPredicateDeterministic sorts contact manifolds based on islandid,
//but also based on object0 unique id and object1 unique id //but also based on object0 unique id and object1 unique id
if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs)
{
m_islandmanifold.quickSort(btPersistentManifoldSortPredicateDeterministic()); m_islandmanifold.quickSort(btPersistentManifoldSortPredicateDeterministic());
} else
{
m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
}
//m_islandmanifold.heapSort(btPersistentManifoldSortPredicate()); //m_islandmanifold.heapSort(btPersistentManifoldSortPredicate());