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

enable btMultiBodyGearConstraint positional error correction

Browse Source 
fix multibody debug drawing of joint frames
This commit is contained in:
Erwin Coumans
2018-05-01 17:40:26 -07:00
parent dde3140987
commit 988d78cb61
4 changed files with 23 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
View File

@@ -1342,9 +1342,12 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
btVector3 axis = tr.getBasis().getColumn(0);
btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0));
if (minTh <= maxTh)
{
btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0));
}
axis = tr.getBasis().getColumn(1);
btScalar ay = p6DOF->getAngle(1);
btScalar az = p6DOF->getAngle(2);

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

@@ -826,21 +826,24 @@ void btMultiBodyDynamicsWorld::debugDrawWorld()
{
btMultiBody* bod = m_multiBodies[b];
bod->forwardKinematics(m_scratch_world_to_local1,m_scratch_local_origin1);
getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
if (mode & btIDebugDraw::DBG_DrawFrames)
{
getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
}
for (int m = 0; m<bod->getNumLinks(); m++)
{
const btTransform& tr = bod->getLink(m).m_cachedWorldTransform;
getDebugDrawer()->drawTransform(tr, 0.1);
if (mode & btIDebugDraw::DBG_DrawFrames)
{
getDebugDrawer()->drawTransform(tr, 0.1);
}
//draw the joint axis
if (bod->getLink(m).m_jointType==btMultibodyLink::eRevolute)
{
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_topVec);
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_topVec)*0.1;
btVector4 color(0,0,0,1);//1,1,1);
btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
@@ -849,7 +852,7 @@ void btMultiBodyDynamicsWorld::debugDrawWorld()
}
if (bod->getLink(m).m_jointType==btMultibodyLink::eFixed)
{
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec)*0.1;
btVector4 color(0,0,0,1);//1,1,1);
btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
@@ -858,7 +861,7 @@ void btMultiBodyDynamicsWorld::debugDrawWorld()
}
if (bod->getLink(m).m_jointType==btMultibodyLink::ePrismatic)
{
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec)*0.1;
btVector4 color(0,0,0,1);//1,1,1);
btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);

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

@@ -134,6 +134,10 @@ void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray&
if (m_erp!=0)
{
btScalar currentPositionA = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
if (m_gearAuxLink >= 0)
{
currentPositionA -= m_bodyA->getJointPosMultiDof(m_gearAuxLink)[dof];
}
btScalar currentPositionB = m_gearRatio*m_bodyA->getJointPosMultiDof(m_linkB)[dof];
btScalar diff = currentPositionB+currentPositionA;
btScalar desiredPositionDiff = this->m_relativePositionTarget;

2
src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp
View File

@@ -157,7 +157,7 @@ void btSoftMultiBodyDynamicsWorld::removeCollisionObject(btCollisionObject* coll
void btSoftMultiBodyDynamicsWorld::debugDrawWorld()
{
btDiscreteDynamicsWorld::debugDrawWorld();
btMultiBodyDynamicsWorld::debugDrawWorld();
if (getDebugDrawer())
{