Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests 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
Download Patch File Download Diff File Expand all files Collapse all files

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