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

Bug in 6DOF constraint fixed

Browse Source 
Twist is now locked when twistSpan == 0 for btConeTwistConstraint
Hinge axis for rigidBodyB is no longer flipped
This commit is contained in:
rponom
2009-02-06 00:04:32 +00:00
parent fd2cc88db8
commit 2766d19d65
4 changed files with 35 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -610,7 +610,7 @@ void btConeTwistConstraint::calcAngleInfo2()
// or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
}
if (m_twistSpan >= btScalar(0.05f))
if (m_twistSpan >= btScalar(0.f))
{
btVector3 twistAxis;
computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis);

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

@@ -19,19 +19,19 @@ email: projectileman@yahoo.com
http://gimpact.sf.net
*/
#include "btGeneric6DofConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btTransformUtil.h"
#include <new>
#define D6_USE_OBSOLETE_METHOD false
//-----------------------------------------------------------------------------
btGeneric6DofConstraint::btGeneric6DofConstraint()
:btTypedConstraint(D6_CONSTRAINT_TYPE),
m_useLinearReferenceFrameA(true),
m_useSolveConstraintObsolete(false)
//m_useSolveConstraintObsolete(true)
m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
{
}
@@ -42,8 +42,7 @@ btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody&
, m_frameInA(frameInA)
, m_frameInB(frameInB),
m_useLinearReferenceFrameA(useLinearReferenceFrameA),
m_useSolveConstraintObsolete(false)
//m_useSolveConstraintObsolete(true)
m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
{
}
@@ -537,7 +536,6 @@ int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info)
int row = 0;
//solve linear limits
btRotationalLimitMotor limot;
bool useBasisBodyB = (m_rbB.getInvMass() == btScalar(0.f));
for (int i=0;i<3 ;i++ )
{
if(m_linearLimits.needApplyForce(i))
@@ -554,15 +552,7 @@ int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info)
limot.m_maxLimitForce = btScalar(0.f);
limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
btVector3 axis;
if(useBasisBodyB)
{
axis = m_calculatedTransformB.getBasis().getColumn(i);
}
else
{
axis = m_calculatedTransformA.getBasis().getColumn(i);
}
btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
row += get_limit_motor_info2(&limot, &m_rbA, &m_rbB, info, row, axis, 0);
}
}

22
src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
View File

@@ -71,9 +71,9 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
m_rbBFrame.getOrigin() = pivotInB;
m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(),
rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(),
rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() );
m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
//start with free
m_lowerLimit = btScalar(1e30);
@@ -102,7 +102,7 @@ m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER)
rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA;
btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA;
btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
@@ -132,11 +132,6 @@ m_angularOnly(false),
m_enableAngularMotor(false),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER)
{
// flip axis
m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
//start with free
m_lowerLimit = btScalar(1e30);
m_upperLimit = btScalar(-1e30);
@@ -156,11 +151,6 @@ m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER)
{
///not providing rigidbody B means implicitly using worldspace for body B
// flip axis
m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
//start with free
@@ -364,7 +354,7 @@ void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
// angular_velocity = (erp*fps) * (ax1 x ax2)
// ax1 x ax2 is in the plane space of ax1, so we project the angular
// velocity to p and q to find the right hand side.
btVector3 ax2 = - trB.getBasis().getColumn(2);
btVector3 ax2 = trB.getBasis().getColumn(2);
btVector3 u = ax1.cross(ax2);
info->m_constraintError[s3] = k * u.dot(p);
info->m_constraintError[s4] = k * u.dot(q);
@@ -551,7 +541,7 @@ void btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody
}
//solve angular positional correction
btVector3 angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep);
btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/timeStep);
btScalar len2 = angularError.length();
if (len2>btScalar(0.00001))
{

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

@@ -1294,12 +1294,18 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
pPrev = pCur;
}
btVector3 pivot = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame().getOrigin();
btScalar tws = pCT->getTwistSpan();
btScalar twa = pCT->getTwistAngle();
bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f));
if(useFrameB)
{
tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
}
else
{
tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
}
btVector3 pivot = tr.getOrigin();
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);
@@ -1324,7 +1330,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, dbgDrawSize, minTh, maxTh, minPs, maxPs, btVector3(0,0,0));
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);
@@ -1340,11 +1346,18 @@ 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;
if(minFi > maxFi)
{
getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0,0,0), false);
}
else if(minFi < maxFi)
{
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;
getDebugDrawer()->drawBox(bbMin, bbMax, btVector3(0,0,0));
btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit;
btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit;
getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0,0,0));
}
}
break;