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

Bug in Generic 6DOF joint fixed

Browse Source 
Debug visualization of joint constraints was added
This commit is contained in:
rponom
2009-01-16 03:04:07 +00:00
parent bdf78e0735
commit e7ca0e6a9c
5 changed files with 245 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
View File

@@ -307,6 +307,7 @@ void btConeTwistConstraint::buildJacobian()
btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
btVector3 TwistRef = quatRotate(rotationArc,b2Axis2);
btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
m_twistAngle = twist;
btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
if (twist <= -m_twistSpan*lockedFreeFactor)
@@ -502,6 +503,7 @@ void btConeTwistConstraint::calcAngleInfo()
btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
btVector3 TwistRef = quatRotate(rotationArc,b2Axis2);
btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
m_twistAngle = twist;
// btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.);

19
src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
View File

@@ -56,6 +56,8 @@ public:
btScalar m_swingCorrection;
btScalar m_twistCorrection;
btScalar m_twistAngle;
btScalar m_accSwingLimitImpulse;
btScalar m_accTwistLimitImpulse;
@@ -129,6 +131,23 @@ public:
void calcAngleInfo();
inline btScalar getSwingSpan1()
{
return m_swingSpan1;
}
inline btScalar getSwingSpan2()
{
return m_swingSpan2;
}
inline btScalar getTwistSpan()
{
return m_twistSpan;
}
inline btScalar getTwistAngle()
{
return m_twistAngle;
}
};

63
src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
View File

@@ -544,7 +544,7 @@ int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info)
{ // re-use rotational motor code
limot.m_bounce = btScalar(0.f);
limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
limot.m_currentLimitError = -m_linearLimits.m_currentLimitError[i];
limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
limot.m_damping = m_linearLimits.m_damping;
limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
limot.m_ERP = m_linearLimits.m_restitution;
@@ -578,7 +578,6 @@ int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_o
//solve angular limits
for (int i=0;i<3 ;i++ )
{
//if(i==2) continue;
if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
{
btVector3 axis = d6constraint->getAxis(i);
@@ -732,54 +731,31 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
J2[srow+1] = -ax1[1];
J2[srow+2] = -ax1[2];
}
// linear limot torque decoupling step:
//
// if this is a linear limot (e.g. from a slider), we have to be careful
// that the linear constraint forces (+/- ax1) applied to the two bodies
// do not create a torque couple. in other words, the points that the
// constraint force is applied at must lie along the same ax1 axis.
// a torque couple will result in powered or limited slider-jointed free
// bodies from gaining angular momentum.
// the solution used here is to apply the constraint forces at the point
// at center of mass of two bodies. there is no penalty (other than an
// extra tiny bit of computation) in doing this adjustment. note that we
// only need to do this if the constraint connects two bodies.
btVector3 ltd; // Linear Torque Decoupling vector (a torque)
if(!rotational)
if((!rotational) && limit)
{
btVector3 c = body1->getCenterOfMassPosition() - body0->getCenterOfMassPosition();
btVector3 ltd; // Linear Torque Decoupling vector
btVector3 c = m_calculatedTransformB.getOrigin() - body0->getCenterOfMassPosition();
ltd = c.cross(ax1);
btScalar miA = body0->getInvMass();
btScalar miB = body1->getInvMass();
btScalar miS = miA + miB;
btScalar factA, factB;
if(miS > btScalar(0.f))
{
factA = miB / miS;
}
else
{
factA = btScalar(0.5f);
}
if(factA > 0.99f) factA = 0.99f;
if(factA < 0.01f) factA = 0.01f;
factB = btScalar(1.0f) - factA;
info->m_J1angularAxis[srow+0] = factA * ltd[0];
info->m_J1angularAxis[srow+1] = factA * ltd[1];
info->m_J1angularAxis[srow+2] = factA * ltd[2];
info->m_J2angularAxis[srow+0] = factB * ltd[0];
info->m_J2angularAxis[srow+1] = factB * ltd[1];
info->m_J2angularAxis[srow+2] = factB * ltd[2];
info->m_J1angularAxis[srow+0] = ltd[0];
info->m_J1angularAxis[srow+1] = ltd[1];
info->m_J1angularAxis[srow+2] = ltd[2];
c = m_calculatedTransformB.getOrigin() - body1->getCenterOfMassPosition();
ltd = -c.cross(ax1);
info->m_J2angularAxis[srow+0] = ltd[0];
info->m_J2angularAxis[srow+1] = ltd[1];
info->m_J2angularAxis[srow+2] = ltd[2];
}
// if we're limited low and high simultaneously, the joint motor is
// ineffective
if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
info->m_constraintError[srow] = btScalar(0.f);
if (powered)
{
info->cfm[srow] = 0.0f;
if(!limit)
{
info->m_constraintError[srow] = limot->m_targetVelocity;
info->m_constraintError[srow] += limot->m_targetVelocity;
info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
info->m_upperLimit[srow] = limot->m_maxMotorForce;
}
@@ -787,7 +763,14 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
if(limit)
{
btScalar k = info->fps * limot->m_ERP;
info->m_constraintError[srow] = -k * limot->m_currentLimitError;
if(!rotational)
{
info->m_constraintError[srow] += k * limot->m_currentLimitError;
}
else
{
info->m_constraintError[srow] += -k * limot->m_currentLimitError;
}
info->cfm[srow] = 0.0f;
if (limot->m_loLimit == limot->m_hiLimit)
{ // limited low and high simultaneously

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

@@ -29,6 +29,11 @@ subject to the following restrictions:
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
//for debug rendering
#include "BulletCollision/CollisionShapes/btBoxShape.h"
@@ -147,6 +152,24 @@ void btDiscreteDynamicsWorld::debugDrawWorld()
}
}
}
bool drawConstraints = false;
if (getDebugDrawer())
{
int mode = getDebugDrawer()->getDebugMode();
if(mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
{
drawConstraints = true;
}
}
if(drawConstraints)
{
for(int i = getNumConstraints()-1; i>=0 ;i--)
{
btTypedConstraint* constraint = getConstraint(i);
debugDrawConstraint(constraint);
}
}
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb))
@@ -1187,6 +1210,182 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform,
}
#define DEBUG_DRAW_CONSTR_LEN btScalar(5.f)
void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
{
bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0;
bool drawLimits = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraintLimits) != 0;
switch(constraint->getConstraintType())
{
case POINT2POINT_CONSTRAINT_TYPE:
{
btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint;
btTransform tr;
tr.setIdentity();
btVector3 pivot = p2pC->getPivotInA();
pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot;
tr.setOrigin(pivot);
getDebugDrawer()->drawTransform(tr, DEBUG_DRAW_CONSTR_LEN);
// that ideally should draw the same frame
pivot = p2pC->getPivotInB();
pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot;
tr.setOrigin(pivot);
if(drawFrames) getDebugDrawer()->drawTransform(tr, DEBUG_DRAW_CONSTR_LEN);
}
break;
case HINGE_CONSTRAINT_TYPE:
{
btHingeConstraint* pHinge = (btHingeConstraint*)constraint;
btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame();
if(drawFrames) getDebugDrawer()->drawTransform(tr, DEBUG_DRAW_CONSTR_LEN);
tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame();
if(drawFrames) getDebugDrawer()->drawTransform(tr, DEBUG_DRAW_CONSTR_LEN);
btScalar minAng = pHinge->getLowerLimit();
btScalar maxAng = pHinge->getUpperLimit();
if(minAng == maxAng)
{
break;
}
bool drawSect = true;
if(minAng > maxAng)
{
minAng = btScalar(0.f);
maxAng = SIMD_2_PI;
drawSect = false;
}
if(drawLimits)
{
btVector3& center = tr.getOrigin();
btVector3& normal = tr.getBasis().getColumn(2);
btVector3& axis = tr.getBasis().getColumn(0);
getDebugDrawer()->drawArc(center, normal, axis, DEBUG_DRAW_CONSTR_LEN, DEBUG_DRAW_CONSTR_LEN, minAng, maxAng, btVector3(0,0,0), drawSect);
}
}
break;
case CONETWIST_CONSTRAINT_TYPE:
{
btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint;
btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
if(drawFrames) getDebugDrawer()->drawTransform(tr, DEBUG_DRAW_CONSTR_LEN);
tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
if(drawFrames) getDebugDrawer()->drawTransform(tr, DEBUG_DRAW_CONSTR_LEN);
if(drawLimits)
{
btScalar sw1 = pCT->getSwingSpan1();
btScalar sw2 = pCT->getSwingSpan2();
btScalar dist = sw1 > sw2 ? sw1 : sw2;
dist = DEBUG_DRAW_CONSTR_LEN * btCos(dist);
sw1 = DEBUG_DRAW_CONSTR_LEN * btSin(sw1);
sw2 = DEBUG_DRAW_CONSTR_LEN * btSin(sw2);
tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
tr.setOrigin(btVector3(0,0,0));
btVector3 center = btVector3(dist, 0.f, 0.f);
center = tr * center;
btVector3& pivot = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame().getOrigin();
center += pivot;
tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
btVector3& normal = tr.getBasis().getColumn(0);
btVector3& axis1 = tr.getBasis().getColumn(1);
btVector3& axis2 = tr.getBasis().getColumn(2);
btVector3 vert = center + axis1 * sw1;
getDebugDrawer()->drawLine(pivot, vert, btVector3(0,0,0));
vert = center - axis1 * sw1;
getDebugDrawer()->drawLine(pivot, vert, btVector3(0,0,0));
vert = center + axis2 * sw2;
getDebugDrawer()->drawLine(pivot, vert, btVector3(0,0,0));
vert = center - axis2 * sw2;
getDebugDrawer()->drawLine(pivot, vert, btVector3(0,0,0));
btScalar minAng = btScalar(0.f);
btScalar maxAng = SIMD_2_PI;
getDebugDrawer()->drawArc(center, normal, axis1, sw1, sw2, minAng, maxAng, btVector3(0,0,0), false);
btScalar tws = pCT->getTwistSpan();
btScalar twa = pCT->getTwistAngle();
tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
normal = tr.getBasis().getColumn(0);
axis1 = tr.getBasis().getColumn(1);
getDebugDrawer()->drawArc(pivot, normal, axis1, DEBUG_DRAW_CONSTR_LEN, DEBUG_DRAW_CONSTR_LEN, -twa-tws, -twa+tws, btVector3(0,0,0), true);
}
}
break;
case D6_CONSTRAINT_TYPE:
{
btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint;
btTransform tr = p6DOF->getCalculatedTransformA();
if(drawFrames) getDebugDrawer()->drawTransform(tr, DEBUG_DRAW_CONSTR_LEN);
tr = p6DOF->getCalculatedTransformB();
if(drawFrames) getDebugDrawer()->drawTransform(tr, DEBUG_DRAW_CONSTR_LEN);
if(drawLimits)
{
tr = p6DOF->getCalculatedTransformA();
const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
btVector3& up = tr.getBasis().getColumn(2);
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, DEBUG_DRAW_CONSTR_LEN, minTh, maxTh, minPs, maxPs, btVector3(0,0,0));
axis = tr.getBasis().getColumn(1);
btScalar ay = p6DOF->getAngle(1);
btScalar az = p6DOF->getAngle(2);
btScalar cy = btCos(ay);
btScalar sy = btSin(ay);
btScalar cz = btCos(az);
btScalar sz = btSin(az);
btVector3 ref;
ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2];
ref[1] = -sz*axis[0] + cz*axis[1];
ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2];
tr = p6DOF->getCalculatedTransformB();
btVector3& normal = -tr.getBasis().getColumn(0);
btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;
btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit;
getDebugDrawer()->drawArc(center, normal, ref, DEBUG_DRAW_CONSTR_LEN, DEBUG_DRAW_CONSTR_LEN, minFi, maxFi, btVector3(0,0,0), true);
tr = p6DOF->getCalculatedTransformA();
btVector3 bbMin = tr * p6DOF->getTranslationalLimitMotor()->m_lowerLimit;
btVector3 bbMax = tr * p6DOF->getTranslationalLimitMotor()->m_upperLimit;
getDebugDrawer()->drawBox(bbMin, bbMax, btVector3(0,0,0));
}
}
break;
case SLIDER_CONSTRAINT_TYPE:
{
btSliderConstraint* pSlider = (btSliderConstraint*)constraint;
btTransform tr = pSlider->getCalculatedTransformA();
if(drawFrames) getDebugDrawer()->drawTransform(tr, DEBUG_DRAW_CONSTR_LEN);
tr = pSlider->getCalculatedTransformB();
if(drawFrames) getDebugDrawer()->drawTransform(tr, DEBUG_DRAW_CONSTR_LEN);
if(drawLimits)
{
btTransform tr = pSlider->getCalculatedTransformA();
btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f);
btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f);
getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0));
btVector3& normal = tr.getBasis().getColumn(0);
btVector3& axis = tr.getBasis().getColumn(1);
btScalar a_min = pSlider->getLowerAngLimit();
btScalar a_max = pSlider->getUpperAngLimit();
const btVector3& center = pSlider->getCalculatedTransformB().getOrigin();
getDebugDrawer()->drawArc(center, normal, axis, DEBUG_DRAW_CONSTR_LEN, DEBUG_DRAW_CONSTR_LEN, a_min, a_max, btVector3(0,0,0), true);
}
}
break;
default :
break;
}
return;
} // btDiscreteDynamicsWorld::debugDrawConstraint()
void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
{
if (m_ownsConstraintSolver)

2
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
View File

@@ -140,6 +140,8 @@ public:
void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
void debugDrawConstraint(btTypedConstraint* constraint);
virtual void debugDrawWorld();
virtual void setConstraintSolver(btConstraintSolver* solver);