AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2025-01-18 21:10:05 +00:00
Code Issues Releases Wiki Activity

Merge pull request #1847 from erwincoumans/master

Browse Source 
Fix some deactivation issues with btMultiBodyDynamicsWorld, should al…
This commit is contained in:
erwincoumans 2018-08-26 18:38:34 -07:00 committed by GitHub
commit 8cac231890
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
20 changed files with 167 additions and 96 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@ -610,9 +610,10 @@ void ConvertURDF2BulletInternal(
}
} else
{
if (canSleep)
// if (canSleep)
{
if (cache.m_bulletMultiBody->getBaseMass()==0 && cache.m_bulletMultiBody->getNumDofs()==0)
if (cache.m_bulletMultiBody->getBaseMass()==0)
//&& cache.m_bulletMultiBody->getNumDofs()==0)
{
//col->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
col->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT);

9
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -629,6 +629,15 @@ B3_SHARED_API int b3PhysicsParameterSetConstraintSolverType(b3SharedMemoryCom
return 0;
}
B3_SHARED_API int b3PhysicsParameterSetMinimumSolverIslandSize(b3SharedMemoryCommandHandle commandHandle, int minimumSolverIslandSize)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_minimumSolverIslandSize = minimumSolverIslandSize;
command->m_updateFlags |= SIM_PARAM_CONSTRAINT_MIN_SOLVER_ISLAND_SIZE;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode)
{

3
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -320,7 +320,8 @@ B3_SHARED_API int b3PhysicsParamSetSolverResidualThreshold(b3SharedMemoryCommand
B3_SHARED_API int b3PhysicsParamSetContactSlop(b3SharedMemoryCommandHandle commandHandle, double contactSlop);
B3_SHARED_API int b3PhysicsParameterSetEnableSAT(b3SharedMemoryCommandHandle commandHandle, int enableSAT);
B3_SHARED_API int b3PhysicsParameterSetConstraintSolverType(b3SharedMemoryCommandHandle commandHandle, int constraintSolverType);
B3_SHARED_API int b3PhysicsParameterSetMinimumSolverIslandSize(b3SharedMemoryCommandHandle commandHandle, int minimumSolverIslandSize);

8
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -2365,9 +2365,11 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
m_data->m_pairCache->setOverlapFilterCallback(m_data->m_broadphaseCollisionFilterCallback);
//int maxProxies = 32768;
int maxProxies = 32768;
//m_data->m_broadphase = new btSimpleBroadphase(maxProxies, m_data->m_pairCache);
m_data->m_broadphase = new btDbvtBroadphase(m_data->m_pairCache);
btDbvtBroadphase* bv = new btDbvtBroadphase(m_data->m_pairCache);
bv->setVelocityPrediction(0);
m_data->m_broadphase = bv;
m_data->m_solver = new btMultiBodyConstraintSolver;
@ -2390,6 +2392,8 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = 0.2;//need to check if there are artifacts with frictionERP
m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 0;
gDbvtMargin = btScalar(0);
m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = 1e-7;
// m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 2;
//todo: islands/constraints are buggy in btMultiBodyDynamicsWorld! (performance + see slipping grasp)

2
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -468,6 +468,8 @@ enum EnumSimParamUpdateFlags
SIM_PARAM_UPDATE_CONTACT_SLOP = 4194304,
SIM_PARAM_ENABLE_SAT = 8388608,
SIM_PARAM_CONSTRAINT_SOLVER_TYPE =16777216,
SIM_PARAM_CONSTRAINT_MIN_SOLVER_ISLAND_SIZE = 33554432,
};
enum EnumLoadSoftBodyUpdateFlags

1
examples/SharedMemory/SharedMemoryPublic.h
View File

@ -898,6 +898,7 @@ struct b3PhysicsSimulationParameters
double m_contactSlop;
int m_enableSAT;
int m_constraintSolverType;
int m_minimumSolverIslandSize;
};
enum eConstraintSolverTypes

13
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
View File

@ -1353,6 +1353,10 @@ bool b3RobotSimulatorClientAPI_NoDirect::changeDynamics(int bodyUniqueId, int li
b3SharedMemoryCommandHandle command = b3InitChangeDynamicsInfo(sm);
b3SharedMemoryStatusHandle statusHandle;
if (args.m_activationState >= 0)
{
b3ChangeDynamicsInfoSetActivationState(command, bodyUniqueId,args.m_activationState);
}
if (args.m_mass >= 0) {
b3ChangeDynamicsInfoSetMass(command, bodyUniqueId, linkIndex, args.m_mass);
}
@ -1714,6 +1718,15 @@ bool b3RobotSimulatorClientAPI_NoDirect::setPhysicsEngineParameter(const struct
b3PhysicsParamSetSolverResidualThreshold(command, args.m_solverResidualThreshold);
}
if (args.m_constraintSolverType >= 0)
{
b3PhysicsParameterSetConstraintSolverType(command, args.m_constraintSolverType);
}
if (args.m_minimumSolverIslandSize >= 0)
{
b3PhysicsParameterSetMinimumSolverIslandSize(command, args.m_minimumSolverIslandSize);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
return true;
}

7
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
View File

@ -268,7 +268,8 @@ struct b3RobotSimulatorSetPhysicsEngineParameters : b3PhysicsSimulationParameter
m_frictionERP=-1;
m_solverResidualThreshold=-1;
m_constraintSolverType = -1;
m_minimumSolverIslandSize = -1;
}
};
@ -285,6 +286,7 @@ struct b3RobotSimulatorChangeDynamicsArgs
double m_contactStiffness;
double m_contactDamping;
int m_frictionAnchor;
int m_activationState;
b3RobotSimulatorChangeDynamicsArgs()
: m_mass(-1),
@ -296,7 +298,8 @@ struct b3RobotSimulatorChangeDynamicsArgs
m_angularDamping(-1),
m_contactStiffness(-1),
m_contactDamping(-1),
m_frictionAnchor(-1)
m_frictionAnchor(-1),
m_activationState(-1)
{}
};

2
examples/pybullet/examples/saveRestoreState.py
View File

@ -12,7 +12,7 @@ logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "saveRestoreTimings
def setupWorld():
p.resetSimulation()
p.setPhysicsEngineParameter(deterministicOverlappingPairs=1)
p.setPhysicsEngineParameter(deterministicOverlappingPairs=1)
p.loadURDF("planeMesh.urdf")
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf",[0,0,10])
for i in range (p.getNumJoints(kukaId)):

14
examples/pybullet/pybullet.c
View File

@ -1469,12 +1469,14 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
int constraintSolverType=-1;
double globalCFM = -1;
int minimumSolverIslandSize = -1;
int physicsClientId = 0;
static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "allowedCcdPenetration", "jointFeedbackMode", "solverResidualThreshold", "contactSlop", "enableSAT", "constraintSolverType", "globalCFM", "physicsClientId", NULL};
static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "allowedCcdPenetration", "jointFeedbackMode", "solverResidualThreshold", "contactSlop", "enableSAT", "constraintSolverType", "globalCFM", "minimumSolverIslandSize", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiidi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
&collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &enableSAT, &constraintSolverType, &globalCFM, &physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiiddi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
&collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &enableSAT, &constraintSolverType, &globalCFM, &minimumSolverIslandSize, &physicsClientId))
{
return NULL;
}
@ -1495,6 +1497,12 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
b3PhysicsParamSetNumSolverIterations(command, numSolverIterations);
}
if (minimumSolverIslandSize >= 0)
{
b3PhysicsParameterSetMinimumSolverIslandSize(command, minimumSolverIslandSize);
}
if (solverResidualThreshold>=0)
{
b3PhysicsParamSetSolverResidualThreshold(command, solverResidualThreshold);

11
src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp
View File

@ -17,7 +17,7 @@ subject to the following restrictions:
#include "btDbvtBroadphase.h"
#include "LinearMath/btThreads.h"
btScalar gDbvtMargin = btScalar(0.05);
//
// Profiling
//
@ -332,12 +332,9 @@ void btDbvtBroadphase::setAabb( btBroadphaseProxy* absproxy,
if(delta[0]<0) velocity[0]=-velocity[0];
if(delta[1]<0) velocity[1]=-velocity[1];
if(delta[2]<0) velocity[2]=-velocity[2];
if (
#ifdef DBVT_BP_MARGIN
m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN)
#else
m_sets[0].update(proxy->leaf,aabb,velocity)
#endif
if (
m_sets[0].update(proxy->leaf, aabb, velocity, gDbvtMargin)
)
{
++m_updates_done;

3
src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h
View File

@ -29,7 +29,8 @@ subject to the following restrictions:
#define DBVT_BP_PREVENTFALSEUPDATE 0
#define DBVT_BP_ACCURATESLEEPING 0
#define DBVT_BP_ENABLE_BENCHMARK 0
#define DBVT_BP_MARGIN (btScalar)0.05
//#define DBVT_BP_MARGIN (btScalar)0.05
extern btScalar gDbvtMargin;
#if DBVT_BP_PROFILE
#define DBVT_BP_PROFILING_RATE 256

3
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
View File

@ -1406,6 +1406,7 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol
btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
{
//printf("btMultiBodyConstraintSolver::solveGroup: numBodies=%d, numConstraints=%d\n", numBodies, numConstraints);
return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
}
@ -1656,6 +1657,8 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
void btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
{
//printf("solveMultiBodyGroup: numBodies=%d, numConstraints=%d, numManifolds=%d, numMultiBodyConstraints=%d\n", numBodies, numConstraints, numManifolds, numMultiBodyConstraints);
//printf("solveMultiBodyGroup start\n");
m_tmpMultiBodyConstraints = multiBodyConstraints;
m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;

2
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
View File

@ -352,7 +352,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
for (i=0;i<numCurMultiBodyConstraints;i++)
m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]);
if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize)
if ((m_multiBodyConstraints.size()+m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize)
{
processConstraints();
} else

28
src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp
View File

@ -65,13 +65,16 @@ int btMultiBodyFixedConstraint::getIslandIdA() const
if (m_bodyA)
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
if (m_linkA < 0)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
}
else
{
if (m_bodyA->getLink(m_linkA).m_collider)
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
}
}
return -1;
@ -83,16 +86,17 @@ int btMultiBodyFixedConstraint::getIslandIdB() const
return m_rigidBodyB->getIslandTag();
if (m_bodyB)
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
if (m_linkB < 0)
{
col = m_bodyB->getLink(i).m_collider;
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
}
else
{
if (m_bodyB->getLink(m_linkB).m_collider)
return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
}
}
return -1;
}

29
src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp
View File

@ -45,16 +45,18 @@ btMultiBodyGearConstraint::~btMultiBodyGearConstraint()
int btMultiBodyGearConstraint::getIslandIdA() const
{
if (m_bodyA)
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
if (m_linkA < 0)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
}
else
{
if (m_bodyA->getLink(m_linkA).m_collider)
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
}
}
return -1;
@ -64,16 +66,17 @@ int btMultiBodyGearConstraint::getIslandIdB() const
{
if (m_bodyB)
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
if (m_linkB < 0)
{
col = m_bodyB->getLink(i).m_collider;
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
}
else
{
if (m_bodyB->getLink(m_linkB).m_collider)
return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
}
}
return -1;
}

34
src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
View File

@ -53,17 +53,22 @@ btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
{
}
int btMultiBodyJointLimitConstraint::getIslandIdA() const
{
if(m_bodyA)
if (m_bodyA)
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
if (m_linkA < 0)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
}
else
{
if (m_bodyA->getLink(m_linkA).m_collider)
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
}
}
return -1;
@ -71,18 +76,19 @@ int btMultiBodyJointLimitConstraint::getIslandIdA() const
int btMultiBodyJointLimitConstraint::getIslandIdB() const
{
if(m_bodyB)
if (m_bodyB)
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
if (m_linkB < 0)
{
col = m_bodyB->getLink(i).m_collider;
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
}
else
{
if (m_bodyB->getLink(m_linkB).m_collider)
return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
}
}
return -1;
}

32
src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
View File

@ -74,29 +74,37 @@ btMultiBodyJointMotor::~btMultiBodyJointMotor()
int btMultiBodyJointMotor::getIslandIdA() const
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
if (this->m_linkA < 0)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
}
else
{
if (m_bodyA->getLink(m_linkA).m_collider)
{
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
}
}
return -1;
}
int btMultiBodyJointMotor::getIslandIdB() const
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
if (m_linkB < 0)
{
col = m_bodyB->getLink(i).m_collider;
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
}
else
{
if (m_bodyB->getLink(m_linkB).m_collider)
{
return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
}
}
return -1;
}

28
src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
View File

@ -64,13 +64,16 @@ int btMultiBodyPoint2Point::getIslandIdA() const
if (m_bodyA)
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
if (m_linkA < 0)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
}
else
{
if (m_bodyA->getLink(m_linkA).m_collider)
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
}
}
return -1;
@ -82,16 +85,17 @@ int btMultiBodyPoint2Point::getIslandIdB() const
return m_rigidBodyB->getIslandTag();
if (m_bodyB)
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
if (m_linkB < 0)
{
col = m_bodyB->getLink(i).m_collider;
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
}
else
{
if (m_bodyB->getLink(m_linkB).m_collider)
return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
}
}
return -1;
}

29
src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp
View File

@ -68,13 +68,16 @@ int btMultiBodySliderConstraint::getIslandIdA() const
if (m_bodyA)
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
if (m_linkA < 0)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
}
else
{
if (m_bodyA->getLink(m_linkA).m_collider)
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
}
}
return -1;
@ -86,20 +89,20 @@ int btMultiBodySliderConstraint::getIslandIdB() const
return m_rigidBodyB->getIslandTag();
if (m_bodyB)
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
if (m_linkB < 0)
{
col = m_bodyB->getLink(i).m_collider;
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
}
else
{
if (m_bodyB->getLink(m_linkB).m_collider)
return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
}
}
return -1;
}
void btMultiBodySliderConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
{
// Convert local points back to world