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

Improved and more stable btConeTwistConstraint (thanks to Edy Boxerman)

Browse Source
This commit is contained in:
rponom
2009-02-04 02:11:45 +00:00
parent daf350168d
commit 2f23237185
5 changed files with 587 additions and 185 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1210,15 +1210,15 @@ 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;
btScalar dbgDrawSize = constraint->getDbgDrawSize();
if(dbgDrawSize <= btScalar(0.f))
{
return;
}
switch(constraint->getConstraintType())
{
@@ -1230,21 +1230,21 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
btVector3 pivot = p2pC->getPivotInA();
pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot;
tr.setOrigin(pivot);
getDebugDrawer()->drawTransform(tr, DEBUG_DRAW_CONSTR_LEN);
getDebugDrawer()->drawTransform(tr, dbgDrawSize);
// 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);
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
}
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);
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame();
if(drawFrames) getDebugDrawer()->drawTransform(tr, DEBUG_DRAW_CONSTR_LEN);
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
btScalar minAng = pHinge->getLowerLimit();
btScalar maxAng = pHinge->getUpperLimit();
if(minAng == maxAng)
@@ -1263,7 +1263,7 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
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);
getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0,0,0), drawSect);
}
}
break;
@@ -1271,44 +1271,38 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
{
btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint;
btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
if(drawFrames) getDebugDrawer()->drawTransform(tr, DEBUG_DRAW_CONSTR_LEN);
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
if(drawFrames) getDebugDrawer()->drawTransform(tr, DEBUG_DRAW_CONSTR_LEN);
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
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;
//const btScalar length = btScalar(5);
const btScalar length = dbgDrawSize;
static int nSegments = 8*4;
btScalar fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)(nSegments-1)/btScalar(nSegments);
btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length);
pPrev = tr * pPrev;
for (int i=0; i<nSegments; i++)
{
fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)i/btScalar(nSegments);
btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length);
pCur = tr * pCur;
getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0,0,0));
if (i%(nSegments/8) == 0)
getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0));
pPrev = pCur;
}
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);
btVector3 normal = tr.getBasis().getColumn(0);
btVector3 axis1 = tr.getBasis().getColumn(1);
getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa-tws, -twa+tws, btVector3(0,0,0), true);
}
}
@@ -1317,9 +1311,9 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
{
btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint;
btTransform tr = p6DOF->getCalculatedTransformA();
if(drawFrames) getDebugDrawer()->drawTransform(tr, DEBUG_DRAW_CONSTR_LEN);
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
tr = p6DOF->getCalculatedTransformB();
if(drawFrames) getDebugDrawer()->drawTransform(tr, DEBUG_DRAW_CONSTR_LEN);
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
if(drawLimits)
{
tr = p6DOF->getCalculatedTransformA();
@@ -1330,7 +1324,7 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
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));
getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize, minTh, maxTh, minPs, maxPs, btVector3(0,0,0));
axis = tr.getBasis().getColumn(1);
btScalar ay = p6DOF->getAngle(1);
btScalar az = p6DOF->getAngle(2);
@@ -1346,7 +1340,7 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
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);
getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0,0,0), true);
tr = p6DOF->getCalculatedTransformA();
btVector3 bbMin = tr * p6DOF->getTranslationalLimitMotor()->m_lowerLimit;
btVector3 bbMax = tr * p6DOF->getTranslationalLimitMotor()->m_upperLimit;
@@ -1358,9 +1352,9 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
{
btSliderConstraint* pSlider = (btSliderConstraint*)constraint;
btTransform tr = pSlider->getCalculatedTransformA();
if(drawFrames) getDebugDrawer()->drawTransform(tr, DEBUG_DRAW_CONSTR_LEN);
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
tr = pSlider->getCalculatedTransformB();
if(drawFrames) getDebugDrawer()->drawTransform(tr, DEBUG_DRAW_CONSTR_LEN);
if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
if(drawLimits)
{
btTransform tr = pSlider->getCalculatedTransformA();
@@ -1372,7 +1366,7 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
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);
getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0,0,0), true);
}
}
break;
@@ -1414,3 +1408,5 @@ const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const
{
return m_constraints[index];
}