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

fix fixed constraint between btMultiBody and btRigidBody

Browse Source
This commit is contained in:
erwincoumans
2018-05-29 16:49:07 -07:00
parent dcc9c4d0d9
commit ec4b3505a4
2 changed files with 9 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
View File

@@ -253,7 +253,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
{ {
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
if (angConstraint) { if (angConstraint) {
denom0 = rb0->getInvMass() + constraintNormalAng.dot(vec); denom0 = constraintNormalAng.dot(solverConstraint.m_angularComponentA);
} }
else { else {
denom0 = rb0->getInvMass() + constraintNormalLin.dot(vec); denom0 = rb0->getInvMass() + constraintNormalLin.dot(vec);
@@ -277,7 +277,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
{ {
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
if (angConstraint) { if (angConstraint) {
denom1 = rb1->getInvMass() + constraintNormalAng.dot(vec); denom1 = constraintNormalAng.dot(-solverConstraint.m_angularComponentB);
} }
else { else {
denom1 = rb1->getInvMass() + constraintNormalLin.dot(vec); denom1 = rb1->getInvMass() + constraintNormalLin.dot(vec);
@@ -315,7 +315,8 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
} }
else if(rb0) else if(rb0)
{ {
rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); rel_vel += rb0->getLinearVelocity().dot(solverConstraint.m_contactNormal1);
rel_vel += rb0->getAngularVelocity().dot(solverConstraint.m_relpos1CrossNormal);
} }
if (multiBodyB) if (multiBodyB)
{ {
@@ -327,7 +328,8 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
} }
else if(rb1) else if(rb1)
{ {
rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); rel_vel += rb1->getLinearVelocity().dot(solverConstraint.m_contactNormal2);
rel_vel += rb1->getAngularVelocity().dot(solverConstraint.m_relpos2CrossNormal);
} }
solverConstraint.m_friction = 0.f;//cp.m_combinedFriction; solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;

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

@@ -246,7 +246,7 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt
{ {
c.m_appliedImpulse = sum; c.m_appliedImpulse = sum;
} }
if (c.m_multiBodyA) if (c.m_multiBodyA)
{ {
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse, c.m_deltaVelAindex, ndofA); applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse, c.m_deltaVelAindex, ndofA);
@@ -1487,27 +1487,12 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv
if (c.m_multiBodyA) if (c.m_multiBodyA)
{ {
c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
if(c.m_multiBodyA->isMultiDof())
{
c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
}
else
{
c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
}
} }
if (c.m_multiBodyB) if (c.m_multiBodyB)
{ {
if(c.m_multiBodyB->isMultiDof()) c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
{
c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
}
else
{
c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
}
} }
#endif #endif