AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2025-01-08 08:30:16 +00:00
Code Issues Releases Wiki Activity

Some fixes to work with constraint angle limits close to -PI or PI

Browse Source
This commit is contained in:
rponom 2009-06-12 01:23:47 +00:00
parent 699ba8f5b2
commit 99f6ff7cf3
11 changed files with 121 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
Demos/ConstraintDemo/ConstraintDemo.cpp
View File

@ -169,10 +169,12 @@ void ConstraintDemo::initPhysics()
slider->setLinearUpperLimit(hiSliderLimit);
//range should be small, otherwise singularities will 'explode' the constraint
slider->setAngularLowerLimit(btVector3(-1.5,0,0));
slider->setAngularUpperLimit(btVector3(1.5,0,0));
// slider->setAngularLowerLimit(btVector3(-1.5,0,0));
// slider->setAngularUpperLimit(btVector3(1.5,0,0));
// slider->setAngularLowerLimit(btVector3(0,0,0));
// slider->setAngularUpperLimit(btVector3(0,0,0));
slider->setAngularLowerLimit(btVector3(-SIMD_PI,0,0));
slider->setAngularUpperLimit(btVector3(1.5,0,0));
slider->getTranslationalLimitMotor()->m_enableMotor[0] = true;
slider->getTranslationalLimitMotor()->m_targetVelocity[0] = -5.0f;
@ -198,7 +200,11 @@ void ConstraintDemo::initPhysics()
spDoorHinge = new btHingeConstraint( *pDoorBody, btPivotA, btAxisA );
spDoorHinge->setLimit( 0.0f, M_PI_2 );
// spDoorHinge->setLimit( 0.0f, M_PI_2 );
// test problem values
// spDoorHinge->setLimit( -M_PI, M_PI*0.8f);
spDoorHinge->setLimit( -M_PI*0.8f, M_PI);
// spDoorHinge->setLimit( 0.0f, 0.0f );
m_dynamicsWorld->addConstraint(spDoorHinge);
spDoorHinge->setDbgDrawSize(btScalar(5.f));

16
Demos/SliderConstraintDemo/SliderConstraintDemo.cpp
View File

@ -213,11 +213,13 @@ void SliderConstraintDemo::initPhysics()
// spSlider1->setUpperLinLimit(15.0F);
// spSlider1->setLowerLinLimit(-10.0F);
// spSlider1->setUpperLinLimit(-10.0F);
spSlider1->setLowerAngLimit(-SIMD_PI / 3.0F);
spSlider1->setUpperAngLimit( SIMD_PI / 3.0F);
#endif
m_dynamicsWorld->addConstraint(spSlider1, true);
spSlider1->setDbgDrawSize(btScalar(5.f));
// add kinematic rigid body A2
worldPos.setValue(0,2,0);
@ -251,8 +253,13 @@ void SliderConstraintDemo::initPhysics()
// spSlider2->setLowerAngLimit(SIMD_PI / 2.0F);
// spSlider2->setUpperAngLimit(-SIMD_PI / 2.0F);
spSlider2->setLowerAngLimit(-SIMD_PI / 2.0F);
spSlider2->setUpperAngLimit(SIMD_PI / 2.0F);
// spSlider2->setLowerAngLimit(-SIMD_PI / 2.0F);
// spSlider2->setUpperAngLimit(SIMD_PI / 2.0F);
spSlider2->setLowerAngLimit(-SIMD_PI);
spSlider2->setUpperAngLimit(SIMD_PI *0.8F);
// spSlider2->setLowerAngLimit(-0.01F);
// spSlider2->setUpperAngLimit(0.01F);
@ -285,6 +292,7 @@ void SliderConstraintDemo::initPhysics()
// spSlider2->setSoftnessLimAng(0.1);
#endif
m_dynamicsWorld->addConstraint(spSlider2, true);
spSlider2->setDbgDrawSize(btScalar(5.f));
#if 1
@ -306,6 +314,8 @@ void SliderConstraintDemo::initPhysics()
btVector3 pivB(-2.5, 0., 0.);
spP2PConst = new btPoint2PointConstraint(*pRbA3, *pRbB3, pivA, pivB);
m_dynamicsWorld->addConstraint(spP2PConst, true);
spP2PConst->setDbgDrawSize(btScalar(5.f));
}
#endif
@ -335,6 +345,8 @@ void SliderConstraintDemo::initPhysics()
spHingeConst->enableAngularMotor(true, 10.0, 0.19);
m_dynamicsWorld->addConstraint(spHingeConst, true);
spHingeConst->setDbgDrawSize(btScalar(5.f));
#endif
} // SliderConstraintDemo::initPhysics()

1
Demos/SliderConstraintDemo/main.cpp
View File

@ -17,6 +17,7 @@ int main(int argc,char** argv)
sliderConstraintDemo->initPhysics();
sliderConstraintDemo->getDynamicsWorld()->setDebugDrawer(&gDebugDrawer);
sliderConstraintDemo->setDebugMode(btIDebugDraw::DBG_DrawConstraints+btIDebugDraw::DBG_DrawConstraintLimits);
return glutmain(argc, argv,640,480,"Slider Constraint Demo. http://www.continuousphysics.com/Bullet/phpBB2/", sliderConstraintDemo);

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

@ -112,7 +112,6 @@ int btRotationalLimitMotor::testLimitValue(btScalar test_value)
m_currentLimit = 0;//Free from violation
return 0;
}
if (test_value < m_loLimit)
{
m_currentLimit = 1;//low limit violation
@ -420,6 +419,7 @@ void btGeneric6DofConstraint::buildAngularJacobian(
bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
{
btScalar angle = m_calculatedAxisAngleDiff[axis_index];
angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
m_angularLimits[axis_index].m_currentPosition = angle;
//test limits
m_angularLimits[axis_index].testLimitValue(angle);

16
src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
View File

@ -65,8 +65,8 @@ public:
m_targetVelocity = 0;
m_maxMotorForce = 0.1f;
m_maxLimitForce = 300.0f;
m_loLimit = -SIMD_INFINITY;
m_hiLimit = SIMD_INFINITY;
m_loLimit = 1.0f;
m_hiLimit = -1.0f;
m_ERP = 0.5f;
m_bounce = 0.0f;
m_damping = 1.0f;
@ -412,16 +412,14 @@ public:
void setAngularLowerLimit(const btVector3& angularLower)
{
m_angularLimits[0].m_loLimit = angularLower.getX();
m_angularLimits[1].m_loLimit = angularLower.getY();
m_angularLimits[2].m_loLimit = angularLower.getZ();
for(int i = 0; i < 3; i++)
m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
}
void setAngularUpperLimit(const btVector3& angularUpper)
{
m_angularLimits[0].m_hiLimit = angularUpper.getX();
m_angularLimits[1].m_hiLimit = angularUpper.getY();
m_angularLimits[2].m_hiLimit = angularUpper.getZ();
for(int i = 0; i < 3; i++)
m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
}
//! Retrieves the angular limit informacion
@ -446,6 +444,8 @@ public:
}
else
{
lo = btNormalizeAngle(lo);
hi = btNormalizeAngle(hi);
m_angularLimits[axis-3].m_loLimit = lo;
m_angularLimits[axis-3].m_hiLimit = hi;
}

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

@ -79,8 +79,8 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
//start with free
m_lowerLimit = btScalar(BT_LARGE_FLOAT);
m_upperLimit = btScalar(-BT_LARGE_FLOAT);
m_lowerLimit = btScalar(1.0f);
m_upperLimit = btScalar(-1.0f);
m_biasFactor = 0.3f;
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
@ -119,8 +119,8 @@ m_useReferenceFrameA(useReferenceFrameA)
rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
//start with free
m_lowerLimit = btScalar(BT_LARGE_FLOAT);
m_upperLimit = btScalar(-BT_LARGE_FLOAT);
m_lowerLimit = btScalar(1.0f);
m_upperLimit = btScalar(-1.0f);
m_biasFactor = 0.3f;
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
@ -139,8 +139,8 @@ m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useReferenceFrameA(useReferenceFrameA)
{
//start with free
m_lowerLimit = btScalar(BT_LARGE_FLOAT);
m_upperLimit = btScalar(-BT_LARGE_FLOAT);
m_lowerLimit = btScalar(1.0f);
m_upperLimit = btScalar(-1.0f);
m_biasFactor = 0.3f;
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
@ -162,8 +162,8 @@ m_useReferenceFrameA(useReferenceFrameA)
m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
//start with free
m_lowerLimit = btScalar(BT_LARGE_FLOAT);
m_upperLimit = btScalar(-BT_LARGE_FLOAT);
m_lowerLimit = btScalar(1.0f);
m_upperLimit = btScalar(-1.0f);
m_biasFactor = 0.3f;
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
@ -648,7 +648,7 @@ btScalar btHingeConstraint::getHingeAngle()
}
#if 0
void btHingeConstraint::testLimit()
{
// Compute limit information
@ -673,8 +673,36 @@ void btHingeConstraint::testLimit()
}
return;
}
#else
void btHingeConstraint::testLimit()
{
// Compute limit information
m_hingeAngle = getHingeAngle();
m_correction = btScalar(0.);
m_limitSign = btScalar(0.);
m_solveLimit = false;
if (m_lowerLimit <= m_upperLimit)
{
m_hingeAngle = btAdjustAngleToLimits(m_hingeAngle, m_lowerLimit, m_upperLimit);
if (m_hingeAngle <= m_lowerLimit)
{
m_correction = (m_lowerLimit - m_hingeAngle);
m_limitSign = 1.0f;
m_solveLimit = true;
}
else if (m_hingeAngle >= m_upperLimit)
{
m_correction = m_upperLimit - m_hingeAngle;
m_limitSign = -1.0f;
m_solveLimit = true;
}
}
return;
}
#endif
static btVector3 vHinge(0, 0, btScalar(1));
void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)

4
src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
View File

@ -129,8 +129,8 @@ public:
void setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
{
m_lowerLimit = low;
m_upperLimit = high;
m_lowerLimit = btNormalizeAngle(low);
m_upperLimit = btNormalizeAngle(high);
m_limitSoftness = _softness;
m_biasFactor = _biasFactor;

1
src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
View File

@ -799,6 +799,7 @@ void btSliderConstraint::testAngLimits(void)
const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));
rot = btAdjustAngleToLimits(rot, m_lowerAngLimit, m_upperAngLimit);
m_angPos = rot;
if(rot < m_lowerAngLimit)
{

4
src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
View File

@ -151,9 +151,9 @@ public:
btScalar getUpperLinLimit() { return m_upperLinLimit; }
void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; }
btScalar getLowerAngLimit() { return m_lowerAngLimit; }
void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = lowerLimit; }
void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = btNormalizeAngle(lowerLimit); }
btScalar getUpperAngLimit() { return m_upperAngLimit; }
void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = upperLimit; }
void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = btNormalizeAngle(upperLimit); }
bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; }
btScalar getSoftnessDirLin() { return m_softnessDirLin; }
btScalar getRestitutionDirLin() { return m_restitutionDirLin; }

27
src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
View File

@ -244,4 +244,31 @@ public:
};
// returns angle in range [-SIMD_2_PI, SIMD_2_PI], closest to one of the limits
// all arguments should be normalized angles (i.e. in range [-SIMD_PI, SIMD_PI])
SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
{
if(angleLowerLimitInRadians >= angleUpperLimitInRadians)
{
return angleInRadians;
}
else if(angleInRadians < angleLowerLimitInRadians)
{
btScalar diffLo = btNormalizeAngle(angleLowerLimitInRadians - angleInRadians); // this is positive
btScalar diffHi = btFabs(btNormalizeAngle(angleUpperLimitInRadians - angleInRadians));
return (diffLo < diffHi) ? angleInRadians : (angleInRadians + SIMD_2_PI);
}
else if(angleInRadians > angleUpperLimitInRadians)
{
btScalar diffHi = btNormalizeAngle(angleInRadians - angleUpperLimitInRadians); // this is positive
btScalar diffLo = btFabs(btNormalizeAngle(angleInRadians - angleLowerLimitInRadians));
return (diffLo < diffHi) ? (angleInRadians - SIMD_2_PI) : angleInRadians;
}
else
{
return angleInRadians;
}
}
#endif //TYPED_CONSTRAINT_H

19
src/LinearMath/btScalar.h
View File

@ -195,6 +195,7 @@ SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2(x, y);
SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return exp(x); }
SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return log(x); }
SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return pow(x,y); }
SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmod(x,y); }
#else
@ -232,6 +233,7 @@ SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y)
SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return expf(x); }
SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return logf(x); }
SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); }
SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); }
#endif
@ -430,5 +432,22 @@ SIMD_FORCE_INLINE double btUnswapEndianDouble(const unsigned char *src)
return d;
}
// returns normalized value in range [-SIMD_PI, SIMD_PI]
SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians)
{
angleInRadians = btFmod(angleInRadians, SIMD_2_PI);
if(angleInRadians < -SIMD_PI)
{
return angleInRadians + SIMD_2_PI;
}
else if(angleInRadians > SIMD_PI)
{
return angleInRadians - SIMD_2_PI;
}
else
{
return angleInRadians;
}
}
#endif //SIMD___SCALAR_H