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

Fix Featherstone btMultiBodyPoint2Point constraint and picking: don't assume body 0 is the fixed body!

Browse Source
This commit is contained in:
erwin.coumans
2013-10-22 21:06:47 +00:00
parent 75260cd0d7
commit f22ceecb82
3 changed files with 11 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -34,6 +34,7 @@ struct btMultiBodyJacobianData
btAlignedObjectArray<btVector3> scratch_v; btAlignedObjectArray<btVector3> scratch_v;
btAlignedObjectArray<btMatrix3x3> scratch_m; btAlignedObjectArray<btMatrix3x3> scratch_m;
btAlignedObjectArray<btSolverBody>* m_solverBodyPool; btAlignedObjectArray<btSolverBody>* m_solverBodyPool;
int m_fixedBodyId;
}; };

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

@@ -765,6 +765,8 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol
{ {
btMultiBodyConstraint* c = m_tmpMultiBodyConstraints[i]; btMultiBodyConstraint* c = m_tmpMultiBodyConstraints[i];
m_data.m_solverBodyPool = &m_tmpSolverBodyPool; m_data.m_solverBodyPool = &m_tmpSolverBodyPool;
m_data.m_fixedBodyId = m_fixedBodyId;
c->createConstraintRows(m_multiBodyNonContactConstraints,m_data, infoGlobal); c->createConstraintRows(m_multiBodyNonContactConstraints,m_data, infoGlobal);
} }

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

@@ -97,7 +97,10 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
constraintRow.m_solverBodyIdB = 0; constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
btVector3 contactNormalOnB(0,0,0); btVector3 contactNormalOnB(0,0,0);
contactNormalOnB[i] = -1; contactNormalOnB[i] = -1;
@@ -107,6 +110,8 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co
btVector3 pivotAworld = m_pivotInA; btVector3 pivotAworld = m_pivotInA;
if (m_rigidBodyA) if (m_rigidBodyA)
{ {
constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA; pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA;
} else } else
{ {
@@ -116,11 +121,13 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co
btVector3 pivotBworld = m_pivotInB; btVector3 pivotBworld = m_pivotInB;
if (m_rigidBodyB) if (m_rigidBodyB)
{ {
constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB; pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB;
} else } else
{ {
if (m_bodyB) if (m_bodyB)
pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
} }
btScalar position = (pivotAworld-pivotBworld).dot(contactNormalOnB); btScalar position = (pivotAworld-pivotBworld).dot(contactNormalOnB);
btScalar relaxation = 1.f; btScalar relaxation = 1.f;