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

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);