Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Enable btMultiBodyJointLimitConstraint, seems to be working for the prismatic joint

Browse Source 
Fix case sensitive issue, thanks to Martin Felis
Fix crash/bug in btMultiBodyConstraint computation
This commit is contained in:
erwin.coumans
2013-10-02 20:14:48 +00:00
parent 36cfbd47a6
commit c2bece5280
7 changed files with 378 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp
View File

@@ -202,7 +202,6 @@ void FeatherstoneMultiBodyDemo::initPhysics()
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
m_dynamicsWorld->addRigidBody(body);//,1,1+2);
}
}
@@ -210,7 +209,7 @@ void FeatherstoneMultiBodyDemo::initPhysics()
}
createFeatherstoneMultiBody(world, 3, btVector3 (20,29.5,-2), true, false);//true);
createFeatherstoneMultiBody(world, 5, btVector3 (20,29.5,-2), true, true);
createFeatherstoneMultiBody(world, 5, btVector3 (0,29.5,-2), false,false);
@@ -277,7 +276,7 @@ void FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMultiBodyDyn
//add some constraint limit
if (usePrismatic)
{
btMultiBodyConstraint* limit = new btMultiBodyJointLimitConstraint(bod,n_links-1,2,2);
btMultiBodyConstraint* limit = new btMultiBodyJointLimitConstraint(bod,n_links-1,2,3);
world->addMultiBodyConstraint(limit);
}
}

3
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@@ -31,7 +31,7 @@
#include "LinearMath/btAlignedObjectArray.h"
#include "btMultibodyLink.h"
#include "btMultiBodyLink.h"
class btMultiBodyLinkCollider;
class btMultiBody
@@ -287,6 +287,7 @@ public:
}
void setCompanionId(int id)
{
//printf("for %p setCompanionId(%d)\n",this, id);
m_companionId = id;
}
private:

38
src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
View File

@@ -36,6 +36,8 @@ protected:
int m_jac_size_both;
int m_pos_offset;
bool m_isUnilateral;
// data block laid out as follows:
// cached impulses. (one per row.)
// jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc)
@@ -44,32 +46,41 @@ protected:
public:
btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows)
btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral)
:m_bodyA(bodyA),
m_bodyB(bodyB),
m_linkA(linkA),
m_linkB(linkB),
m_num_rows(numRows)
m_num_rows(numRows),
m_isUnilateral(isUnilateral)
{
m_jac_size_A = (6 + bodyA->getNumLinks());
m_jac_size_both = (m_jac_size_A + (bodyB ? 6 + bodyB->getNumLinks() : 0));
m_pos_offset = ((1 + m_jac_size_both)*m_num_rows);
m_data.resize((2 + m_jac_size_both) * m_num_rows);
}
virtual int getIslandIdA() const
virtual int getIslandIdA() const =0;
virtual int getIslandIdB() const =0;
virtual void update()=0;
int getNumRows() const
{
return 0;
}
virtual int getIslandIdB() const
{
return 0;
return m_num_rows;
}
virtual void update()=0;
btMultiBody* getMultiBodyA()
{
return m_bodyA;
}
btMultiBody* getMultiBodyB()
{
return m_bodyB;
}
// current constraint position
// constraint is pos >= 0 for unilateral, or pos = 0 for bilateral
// NOTE: position ignored for friction rows.
// NOTE: ignored position for friction rows.
btScalar getPosition(int row) const
{
return m_data[m_pos_offset + row];
@@ -80,6 +91,12 @@ public:
m_data[m_pos_offset + row] = pos;
}
bool isUnilateral() const
{
return m_isUnilateral;
}
// jacobian blocks.
// each of size 6 + num_links. (jacobian2 is null if no body2.)
// format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients.
@@ -100,6 +117,7 @@ public:
return &m_data[m_num_rows + (row * m_jac_size_both) + m_jac_size_A];
}
};

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

@@ -28,11 +28,12 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
//solve featherstone non-contact constraints
//printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size());
for (int j=0;j<m_multiBodyNonContactConstraints.size();j++)
{
btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[j];
if (iteration < constraint.m_overrideNumSolverIterations)
resolveSingleConstraintRowGeneric(constraint);
//if (iteration < constraint.m_overrideNumSolverIterations)
resolveSingleConstraintRowGenericMultiBody(constraint);
}
//solve featherstone normal contact
@@ -166,6 +167,61 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
}
void btMultiBodyConstraintSolver::resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c)
{
btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
btScalar deltaVelADotn=0;
btScalar deltaVelBDotn=0;
int ndofA=0;
int ndofB=0;
if (c.m_multiBodyA)
{
ndofA = c.m_multiBodyA->getNumLinks() + 6;
for (int i = 0; i < ndofA; ++i)
deltaVelADotn += m_jacobians[c.m_jacAindex+i] * m_deltaVelocities[c.m_deltaVelAindex+i];
}
if (c.m_multiBodyB)
{
ndofB = c.m_multiBodyB->getNumLinks() + 6;
for (int i = 0; i < ndofB; ++i)
deltaVelBDotn += m_jacobians[c.m_jacBindex+i] * m_deltaVelocities[c.m_deltaVelBindex+i];
}
deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv;
const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
if (sum < c.m_lowerLimit)
{
deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
c.m_appliedImpulse = c.m_lowerLimit;
}
else if (sum > c.m_upperLimit)
{
deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
c.m_appliedImpulse = c.m_upperLimit;
}
else
{
c.m_appliedImpulse = sum;
}
if (c.m_multiBodyA)
{
applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
c.m_multiBodyA->applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
}
if (c.m_multiBodyB)
{
applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
c.m_multiBodyB->applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
}
}
void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint,
const btVector3& contactNormal,
@@ -190,8 +246,6 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
// btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
// btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
if (bodyA)
rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
if (bodyB)
@@ -201,7 +255,6 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
if (multiBodyA)
{
//solverConstraint.m_jacA =
const int ndofA = multiBodyA->getNumLinks() + 6;
solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
@@ -436,18 +489,6 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
erp = infoGlobal.m_erp;
}
// const btScalar ALLOWED_PENETRATION = btScalar(0.01);
// float baumgarte_coeff = 0.3;
/// float one_over_dt = 1.f/infoGlobal.m_timeStep;
// btScalar minus_vnew = -penetration * baumgarte_coeff * one_over_dt;
// float myrhs = minus_vnew*solverConstraint.m_jacDiagABInv;//??
// solverConstraint.m_rhs = minus_vnew*solverConstraint.m_jacDiagABInv;//??
//solverConstraint.m_rhsPenetration = 0.f;
//penetration=0.f;
#if 1
if (penetration>0)
{
positionalError = 0;
@@ -464,9 +505,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
//combine position and velocity into rhs
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;//-solverConstraint.m_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv;
//solverConstraint.m_rhs = velocityImpulse;//-solverConstraint.m_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv;
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
solverConstraint.m_rhsPenetration = 0.f;
} else
@@ -475,7 +514,6 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_rhsPenetration = penetrationImpulse;
}
#endif
solverConstraint.m_cfm = 0.f;
solverConstraint.m_lowerLimit = 0;
@@ -484,6 +522,260 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
}
void btMultiBodyConstraintSolver::setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow,
btScalar* jacOrgA,btScalar* jacOrgB,
btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,
const btContactSolverInfo& infoGlobal)
{
BT_PROFILE("setupMultiBodyContactConstraint");
btMultiBody* multiBodyA = constraintRow.m_multiBodyA;
btMultiBody* multiBodyB = constraintRow.m_multiBodyB;
if (multiBodyA)
{
const int ndofA = multiBodyA->getNumLinks() + 6;
constraintRow.m_deltaVelAindex = multiBodyA->getCompanionId();
if (constraintRow.m_deltaVelAindex <0)
{
constraintRow.m_deltaVelAindex = m_deltaVelocities.size();
multiBodyA->setCompanionId(constraintRow.m_deltaVelAindex);
m_deltaVelocities.resize(m_deltaVelocities.size()+ndofA);
} else
{
btAssert(m_deltaVelocities.size() >= constraintRow.m_deltaVelAindex+ndofA);
}
constraintRow.m_jacAindex = m_jacobians.size();
m_jacobians.resize(m_jacobians.size()+ndofA);
m_deltaVelocitiesUnitImpulse.resize(m_deltaVelocitiesUnitImpulse.size()+ndofA);
btAssert(m_jacobians.size() == m_deltaVelocitiesUnitImpulse.size());
for (int i=0;i<ndofA;i++)
m_jacobians[constraintRow.m_jacAindex+i] = jacOrgA[i];
float* delta = &m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
multiBodyA->calcAccelerationDeltas(&m_jacobians[constraintRow.m_jacAindex],delta,scratch_r, scratch_v);
}
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumLinks() + 6;
constraintRow.m_deltaVelBindex = multiBodyB->getCompanionId();
if (constraintRow.m_deltaVelBindex <0)
{
constraintRow.m_deltaVelBindex = m_deltaVelocities.size();
multiBodyB->setCompanionId(constraintRow.m_deltaVelBindex);
m_deltaVelocities.resize(m_deltaVelocities.size()+ndofB);
}
constraintRow.m_jacBindex = m_jacobians.size();
m_jacobians.resize(m_jacobians.size()+ndofB);
for (int i=0;i<ndofB;i++)
m_jacobians[constraintRow.m_jacBindex+i] = jacOrgB[i];
m_deltaVelocitiesUnitImpulse.resize(m_deltaVelocitiesUnitImpulse.size()+ndofB);
btAssert(m_jacobians.size() == m_deltaVelocitiesUnitImpulse.size());
multiBodyB->calcAccelerationDeltas(&m_jacobians[constraintRow.m_jacBindex],&m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],scratch_r, scratch_v);
}
{
btVector3 vec;
btScalar denom0 = 0.f;
btScalar denom1 = 0.f;
btScalar* jacB = 0;
btScalar* jacA = 0;
btScalar* lambdaA =0;
btScalar* lambdaB =0;
int ndofA = 0;
if (multiBodyA)
{
ndofA = multiBodyA->getNumLinks() + 6;
jacA = &m_jacobians[constraintRow.m_jacAindex];
lambdaA = &m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
for (int i = 0; i < ndofA; ++i)
{
float j = jacA[i] ;
float l =lambdaA[i];
denom0 += j*l;
}
}
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumLinks() + 6;
jacB = &m_jacobians[constraintRow.m_jacBindex];
lambdaB = &m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex];
for (int i = 0; i < ndofB; ++i)
{
float j = jacB[i] ;
float l =lambdaB[i];
denom1 += j*l;
}
}
if (multiBodyA && (multiBodyA==multiBodyB))
{
// ndof1 == ndof2 in this case
for (int i = 0; i < ndofA; ++i)
{
denom1 += jacB[i] * lambdaA[i];
denom1 += jacA[i] * lambdaB[i];
}
}
float d = denom0+denom1;
if (btFabs(d)>SIMD_EPSILON)
{
constraintRow.m_jacDiagABInv = 1.f/(d);
} else
{
constraintRow.m_jacDiagABInv = 1.f;
}
}
//compute rhs and remaining constraintRow fields
btScalar restitution = 0.f;
btScalar rel_vel = 0.f;
int ndofA = 0;
int ndofB = 0;
{
btVector3 vel1,vel2;
if (multiBodyA)
{
ndofA = multiBodyA->getNumLinks() + 6;
btScalar* jacA = &m_jacobians[constraintRow.m_jacAindex];
for (int i = 0; i < ndofA ; ++i)
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
}
if (multiBodyB)
{
ndofB = multiBodyB->getNumLinks() + 6;
btScalar* jacB = &m_jacobians[constraintRow.m_jacBindex];
for (int i = 0; i < ndofB ; ++i)
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
}
constraintRow.m_friction = combinedFrictionCoeff;
restitution = restitutionCurve(rel_vel, combinedRestitutionCoeff);
if (restitution <= btScalar(0.))
{
restitution = 0.f;
};
}
/*
///warm starting (or zero if disabled)
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
constraintRow.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
if (constraintRow.m_appliedImpulse)
{
if (multiBodyA)
{
btScalar impulse = constraintRow.m_appliedImpulse;
btScalar* deltaV = &m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
multiBodyA->applyDeltaVee(deltaV,impulse);
applyDeltaVee(deltaV,impulse,constraintRow.m_deltaVelAindex,ndofA);
}
if (multiBodyB)
{
btScalar impulse = constraintRow.m_appliedImpulse;
btScalar* deltaV = &m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex];
multiBodyB->applyDeltaVee(deltaV,impulse);
applyDeltaVee(deltaV,impulse,constraintRow.m_deltaVelBindex,ndofB);
}
}
}
else
*/
{
constraintRow.m_appliedImpulse = 0.f;
}
constraintRow.m_appliedPushImpulse = 0.f;
{
btScalar positionalError = 0.f;
btScalar velocityError = restitution - rel_vel;// * damping;
btScalar erp = infoGlobal.m_erp2;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
erp = infoGlobal.m_erp;
}
// const btScalar ALLOWED_PENETRATION = btScalar(0.01);
// float baumgarte_coeff = 0.3;
/// float one_over_dt = 1.f/infoGlobal.m_timeStep;
// btScalar minus_vnew = -penetration * baumgarte_coeff * one_over_dt;
// float myrhs = minus_vnew*solverConstraint.m_jacDiagABInv;//??
// solverConstraint.m_rhs = minus_vnew*solverConstraint.m_jacDiagABInv;//??
//solverConstraint.m_rhsPenetration = 0.f;
//penetration=0.f;
if (penetration>0)
{
positionalError = 0;
velocityError = -penetration / infoGlobal.m_timeStep;
} else
{
positionalError = -penetration * erp/infoGlobal.m_timeStep;
}
btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
//combine position and velocity into rhs
constraintRow.m_rhs = penetrationImpulse+velocityImpulse;
constraintRow.m_rhsPenetration = 0.f;
} else
{
//split position and velocity into rhs and m_rhsPenetration
constraintRow.m_rhs = velocityImpulse;
constraintRow.m_rhsPenetration = penetrationImpulse;
}
constraintRow.m_cfm = 0.f;
constraintRow.m_lowerLimit = 0;
constraintRow.m_upperLimit = 1e10f;
}
}
btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
{
BT_PROFILE("addMultiBodyFrictionConstraint");
@@ -717,12 +1009,24 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol
//also convert the multibody constraints, if any
for (int i=0;i<m_tmpNumMultiBodyConstraints;i++)
{
btMultiBodyConstraint* c = m_tmpMultiBodyConstraints[i];
c->update();
for (int row=0;row<c->getNumRows();row++)
{
btMultiBodySolverConstraint& constraintRow = m_multiBodyNonContactConstraints.expandNonInitializing();
constraintRow.m_multiBodyA = c->getMultiBodyA();
constraintRow.m_multiBodyB = c->getMultiBodyB();
btScalar penetration = c->getPosition(row);//rhs = c->computeRhs(row,infoGlobal.m_timeStep);
setupMultiBodyJointLimitConstraint(constraintRow,c->jacobianA(row),c->jacobianB(row),penetration,0,0,infoGlobal);
}
}
}
@@ -736,6 +1040,7 @@ btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int
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 start\n");
m_tmpMultiBodyConstraints = multiBodyConstraints;
m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;

7
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h
View File

@@ -48,10 +48,17 @@ protected:
int m_tmpNumMultiBodyConstraints;
void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c);
void resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c);
void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal);
btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0);
void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow,
btScalar* jacA,btScalar* jacB,
btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,
const btContactSolverInfo& infoGlobal);
void setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint,
const btVector3& contactNormal,
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,

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

@@ -273,14 +273,6 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
int i;
//find the first constraint for this island
for (i=0;i<m_numMultiBodyConstraints;i++)
{
if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
{
startMultiBodyConstraint = &m_multiBodySortedConstraints[i];
break;
}
}
for (i=0;i<m_numConstraints;i++)
{
@@ -290,9 +282,6 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
break;
}
}
//count the number of constraints in this island
for (;i<m_numConstraints;i++)
{
@@ -302,6 +291,15 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
}
}
for (i=0;i<m_numMultiBodyConstraints;i++)
{
if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
{
startMultiBodyConstraint = &m_multiBodySortedConstraints[i];
break;
}
}
//count the number of multi body constraints in this island
for (;i<m_numMultiBodyConstraints;i++)
{
@@ -323,6 +321,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
m_manifolds.push_back(manifolds[i]);
for (i=0;i<numCurConstraints;i++)
m_constraints.push_back(startConstraint[i]);
for (i=0;i<numCurMultiBodyConstraints;i++)
m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]);
@@ -348,7 +347,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
m_bodies.resize(0);
m_manifolds.resize(0);
m_constraints.resize(0);
m_multiBodyConstraints.resize(0);
}
};

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

@@ -5,7 +5,7 @@
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
:btMultiBodyConstraint(body,body,link,link,2),
:btMultiBodyConstraint(body,body,link,link,2,true),
m_lowerBound(lower),
m_upperBound(upper)
{