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

improvement for btHingeConstraint to deal with large constraint limits, see Issue 479

Browse Source 
Thanks promyclon for the report and patch, and Roman Ponomarev for testing.
This commit is contained in:
erwin.coumans
2011-02-19 20:02:19 +00:00
parent 48e89d44dc
commit f19995aeab
4 changed files with 270 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -43,6 +43,9 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA),
m_flags(0)
#ifdef _BT_USE_CENTER_LIMIT_
,m_limit()
#endif
{
m_rbAFrame.getOrigin() = pivotInA;
@@ -75,6 +78,7 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
#ifndef _BT_USE_CENTER_LIMIT_
//start with free
m_lowerLimit = btScalar(1.0f);
m_upperLimit = btScalar(-1.0f);
@@ -82,6 +86,7 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
#endif
m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
}
@@ -93,6 +98,9 @@ m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA),
m_flags(0)
#ifdef _BT_USE_CENTER_LIMIT_
,m_limit()
#endif
{
// since no frame is given, assume this to be zero angle and just pick rb transform axis
@@ -117,6 +125,7 @@ m_flags(0)
rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
#ifndef _BT_USE_CENTER_LIMIT_
//start with free
m_lowerLimit = btScalar(1.0f);
m_upperLimit = btScalar(-1.0f);
@@ -124,6 +133,7 @@ m_flags(0)
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
#endif
m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
}
@@ -138,7 +148,11 @@ m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA),
m_flags(0)
#ifdef _BT_USE_CENTER_LIMIT_
,m_limit()
#endif
{
#ifndef _BT_USE_CENTER_LIMIT_
//start with free
m_lowerLimit = btScalar(1.0f);
m_upperLimit = btScalar(-1.0f);
@@ -146,6 +160,7 @@ m_flags(0)
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
#endif
m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
}
@@ -159,11 +174,14 @@ m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA),
m_flags(0)
#ifdef _BT_USE_CENTER_LIMIT_
,m_limit()
#endif
{
///not providing rigidbody B means implicitly using worldspace for body B
m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
#ifndef _BT_USE_CENTER_LIMIT_
//start with free
m_lowerLimit = btScalar(1.0f);
m_upperLimit = btScalar(-1.0f);
@@ -171,6 +189,7 @@ m_flags(0)
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
#endif
m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
}
@@ -449,8 +468,13 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
int limit = 0;
if(getSolveLimit())
{
#ifdef _BT_USE_CENTER_LIMIT_
limit_err = m_limit.getCorrection() * m_referenceSign;
#else
limit_err = m_correction * m_referenceSign;
#endif
limit = (limit_err > btScalar(0.0)) ? 1 : 2;
}
// if the hinge has joint limits or motor, add in the extra row
int powered = 0;
@@ -514,7 +538,11 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
info->m_upperLimit[srow] = 0;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
#ifdef _BT_USE_CENTER_LIMIT_
btScalar bounce = m_limit.getRelaxationFactor();
#else
btScalar bounce = m_relaxationFactor;
#endif
if(bounce > btScalar(0.0))
{
btScalar vel = angVelA.dot(ax1);
@@ -544,7 +572,11 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
}
}
}
#ifdef _BT_USE_CENTER_LIMIT_
info->m_constraintError[srow] *= m_limit.getBiasFactor();
#else
info->m_constraintError[srow] *= m_biasFactor;
#endif
} // if(limit)
} // if angular limit or powered
}
@@ -581,38 +613,14 @@ btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTran
}
#if 0
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)
{
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;
}
#else
void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB)
{
// Compute limit information
m_hingeAngle = getHingeAngle(transA,transB);
#ifdef _BT_USE_CENTER_LIMIT_
m_limit.test(m_hingeAngle);
#else
m_correction = btScalar(0.);
m_limitSign = btScalar(0.);
m_solveLimit = false;
@@ -632,9 +640,10 @@ void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& t
m_solveLimit = true;
}
}
#endif
return;
}
#endif
static btVector3 vHinge(0, 0, btScalar(1));
@@ -665,6 +674,9 @@ void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
{
#ifdef _BT_USE_CENTER_LIMIT_
m_limit.fit(targetAngle);
#else
if (m_lowerLimit < m_upperLimit)
{
if (targetAngle < m_lowerLimit)
@@ -672,7 +684,7 @@ void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
else if (targetAngle > m_upperLimit)
targetAngle = m_upperLimit;
}
#endif
// compute angular velocity
btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
btScalar dAngle = targetAngle - curAngle;
@@ -843,8 +855,13 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
int limit = 0;
if(getSolveLimit())
{
#ifdef _BT_USE_CENTER_LIMIT_
limit_err = m_limit.getCorrection() * m_referenceSign;
#else
limit_err = m_correction * m_referenceSign;
#endif
limit = (limit_err > btScalar(0.0)) ? 1 : 2;
}
// if the hinge has joint limits or motor, add in the extra row
int powered = 0;
@@ -908,7 +925,11 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
info->m_upperLimit[srow] = 0;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
#ifdef _BT_USE_CENTER_LIMIT_
btScalar bounce = m_limit.getRelaxationFactor();
#else
btScalar bounce = m_relaxationFactor;
#endif
if(bounce > btScalar(0.0))
{
btScalar vel = angVelA.dot(ax1);
@@ -938,7 +959,11 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
}
}
}
#ifdef _BT_USE_CENTER_LIMIT_
info->m_constraintError[srow] *= m_limit.getBiasFactor();
#else
info->m_constraintError[srow] *= m_biasFactor;
#endif
} // if(limit)
} // if angular limit or powered
}

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

@@ -17,6 +17,8 @@ subject to the following restrictions:
#ifndef HINGECONSTRAINT_H
#define HINGECONSTRAINT_H
#define _BT_USE_CENTER_LIMIT_ 1
#include "LinearMath/btVector3.h"
#include "btJacobianEntry.h"
@@ -33,6 +35,7 @@ class btRigidBody;
#endif //BT_USE_DOUBLE_PRECISION
enum btHingeFlags
{
BT_HINGE_FLAGS_CFM_STOP = 1,
@@ -57,17 +60,24 @@ public:
btScalar m_motorTargetVelocity;
btScalar m_maxMotorImpulse;
#ifdef _BT_USE_CENTER_LIMIT_
btAngularLimit m_limit;
#else
btScalar m_lowerLimit;
btScalar m_upperLimit;
btScalar m_limitSign;
btScalar m_correction;
btScalar m_limitSoftness;
btScalar m_biasFactor;
btScalar m_relaxationFactor;
btScalar m_lowerLimit;
btScalar m_upperLimit;
bool m_solveLimit;
#endif
btScalar m_kHinge;
btScalar m_limitSign;
btScalar m_correction;
btScalar m_accLimitImpulse;
btScalar m_hingeAngle;
@@ -75,7 +85,6 @@ public:
bool m_angularOnly;
bool m_enableAngularMotor;
bool m_solveLimit;
bool m_useSolveConstraintObsolete;
bool m_useOffsetForConstraintFrame;
bool m_useReferenceFrameA;
@@ -169,13 +178,15 @@ public:
void setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
{
#ifdef _BT_USE_CENTER_LIMIT_
m_limit.set(low, high, _softness, _biasFactor, _relaxationFactor);
#else
m_lowerLimit = btNormalizeAngle(low);
m_upperLimit = btNormalizeAngle(high);
m_limitSoftness = _softness;
m_biasFactor = _biasFactor;
m_relaxationFactor = _relaxationFactor;
#endif
}
void setAxis(btVector3& axisInA)
@@ -202,12 +213,20 @@ public:
btScalar getLowerLimit() const
{
#ifdef _BT_USE_CENTER_LIMIT_
return m_limit.getLow();
#else
return m_lowerLimit;
#endif
}
btScalar getUpperLimit() const
{
#ifdef _BT_USE_CENTER_LIMIT_
return m_limit.getHigh();
#else
return m_upperLimit;
#endif
}
@@ -226,12 +245,20 @@ public:
inline int getSolveLimit()
{
#ifdef _BT_USE_CENTER_LIMIT_
return m_limit.isLimit();
#else
return m_solveLimit;
#endif
}
inline btScalar getLimitSign()
{
#ifdef _BT_USE_CENTER_LIMIT_
return m_limit.getSign();
#else
return m_limitSign;
#endif
}
inline bool getAngularOnly()
@@ -330,12 +357,19 @@ SIMD_FORCE_INLINE const char* btHingeConstraint::serialize(void* dataBuffer, btS
hingeData->m_maxMotorImpulse = float(m_maxMotorImpulse);
hingeData->m_motorTargetVelocity = float(m_motorTargetVelocity);
hingeData->m_useReferenceFrameA = m_useReferenceFrameA;
#ifdef _BT_USE_CENTER_LIMIT_
hingeData->m_lowerLimit = float(m_limit.getLow());
hingeData->m_upperLimit = float(m_limit.getHigh());
hingeData->m_limitSoftness = float(m_limit.getSoftness());
hingeData->m_biasFactor = float(m_limit.getBiasFactor());
hingeData->m_relaxationFactor = float(m_limit.getRelaxationFactor());
#else
hingeData->m_lowerLimit = float(m_lowerLimit);
hingeData->m_upperLimit = float(m_upperLimit);
hingeData->m_limitSoftness = float(m_limitSoftness);
hingeData->m_biasFactor = float(m_biasFactor);
hingeData->m_relaxationFactor = float(m_relaxationFactor);
#endif
return btHingeConstraintDataName;
}

68
src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
View File

@@ -140,3 +140,71 @@ btRigidBody& btTypedConstraint::getFixedBody()
return s_fixed;
}
void btAngularLimit::set(btScalar low, btScalar high, btScalar _softness, btScalar _biasFactor, btScalar _relaxationFactor)
{
m_halfRange = (high - low) / 2.0f;
m_center = btNormalizeAngle(low + m_halfRange);
m_softness = _softness;
m_biasFactor = _biasFactor;
m_relaxationFactor = _relaxationFactor;
}
void btAngularLimit::test(const btScalar angle)
{
m_correction = 0.0f;
m_sign = 0.0f;
m_solveLimit = false;
if (m_halfRange >= 0.0f)
{
btScalar deviation = btNormalizeAngle(angle - m_center);
if (deviation < -m_halfRange)
{
m_solveLimit = true;
m_correction = - (deviation + m_halfRange);
m_sign = +1.0f;
}
else if (deviation > m_halfRange)
{
m_solveLimit = true;
m_correction = m_halfRange - deviation;
m_sign = -1.0f;
}
}
}
btScalar btAngularLimit::getError() const
{
return m_correction * m_sign;
}
void btAngularLimit::fit(btScalar& angle) const
{
if (m_halfRange > 0.0f)
{
btScalar relativeAngle = btNormalizeAngle(angle - m_center);
if (!btEqual(relativeAngle, m_halfRange))
{
if (relativeAngle > 0.0f)
{
angle = getHigh();
}
else
{
angle = getLow();
}
}
}
}
btScalar btAngularLimit::getLow() const
{
return btNormalizeAngle(m_center - m_halfRange);
}
btScalar btAngularLimit::getHigh() const
{
return btNormalizeAngle(m_center + m_halfRange);
}

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

@@ -313,5 +313,98 @@ SIMD_FORCE_INLINE int btTypedConstraint::calculateSerializeBufferSize() const
class btAngularLimit
{
private:
btScalar
m_center,
m_halfRange,
m_softness,
m_biasFactor,
m_relaxationFactor,
m_correction,
m_sign;
bool
m_solveLimit;
public:
/// Default constructor initializes limit as inactive, allowing free constraint movement
btAngularLimit()
:m_center(0.0f),
m_halfRange(-1.0f),
m_softness(0.9f),
m_biasFactor(0.3f),
m_relaxationFactor(1.0f),
m_correction(0.0f),
m_sign(0.0f),
m_solveLimit(false)
{}
/// Sets all limit's parameters.
/// When low > high limit becomes inactive.
/// When high - low > 2PI limit is ineffective too becouse no angle can exceed the limit
void set(btScalar low, btScalar high, btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f);
/// Checks conastaint angle against limit. If limit is active and the angle violates the limit
/// correction is calculated.
void test(const btScalar angle);
/// Returns limit's softness
inline btScalar getSoftness() const
{
return m_softness;
}
/// Returns limit's bias factor
inline btScalar getBiasFactor() const
{
return m_biasFactor;
}
/// Returns limit's relaxation factor
inline btScalar getRelaxationFactor() const
{
return m_relaxationFactor;
}
/// Returns correction value evaluated when test() was invoked
inline btScalar getCorrection() const
{
return m_correction;
}
/// Returns sign value evaluated when test() was invoked
inline btScalar getSign() const
{
return m_sign;
}
/// Gives half of the distance between min and max limit angle
inline btScalar getHalfRange() const
{
return m_halfRange;
}
/// Returns true when the last test() invocation recognized limit violation
inline bool isLimit() const
{
return m_solveLimit;
}
/// Checks given angle against limit. If limit is active and angle doesn't fit it, the angle
/// returned is modified so it equals to the limit closest to given angle.
void fit(btScalar& angle) const;
/// Returns correction value multiplied by sign value
btScalar getError() const;
inline btScalar getLow() const;
inline btScalar getHigh() const;
};
#endif //TYPED_CONSTRAINT_H