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

Auto limitation of spring stiffness and damping in btGeneric6DofSpring2Constraint is now optional.

Browse Source 
Fix: spring stiffness limitation used the mass incorrectly in btGeneric6DofSpring2Constraint.
This commit is contained in:
Gabor Puhr
2015-05-22 12:58:28 +02:00
parent a34d04d211
commit 98e1334a64
4 changed files with 290 additions and 248 deletions
Download Patch File Download Diff File Expand all files Collapse all files

64
src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
View File

@@ -495,22 +495,24 @@ int btGeneric6DofSpring2Constraint::setLinearLimits(btConstraintInfo2* info, int
{
if(m_linearLimits.m_currentLimit[i] || m_linearLimits.m_enableMotor[i] || m_linearLimits.m_enableSpring[i])
{ // re-use rotational motor code
limot.m_bounce = m_linearLimits.m_bounce[i];
limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
limot.m_currentLimitErrorHi = m_linearLimits.m_currentLimitErrorHi[i];
limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
limot.m_servoMotor = m_linearLimits.m_servoMotor[i];
limot.m_servoTarget = m_linearLimits.m_servoTarget[i];
limot.m_enableSpring = m_linearLimits.m_enableSpring[i];
limot.m_springStiffness = m_linearLimits.m_springStiffness[i];
limot.m_springDamping = m_linearLimits.m_springDamping[i];
limot.m_equilibriumPoint = m_linearLimits.m_equilibriumPoint[i];
limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
limot.m_bounce = m_linearLimits.m_bounce[i];
limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
limot.m_currentLimitErrorHi = m_linearLimits.m_currentLimitErrorHi[i];
limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
limot.m_servoMotor = m_linearLimits.m_servoMotor[i];
limot.m_servoTarget = m_linearLimits.m_servoTarget[i];
limot.m_enableSpring = m_linearLimits.m_enableSpring[i];
limot.m_springStiffness = m_linearLimits.m_springStiffness[i];
limot.m_springStiffnessLimited = m_linearLimits.m_springStiffnessLimited[i];
limot.m_springDamping = m_linearLimits.m_springDamping[i];
limot.m_springDampingLimited = m_linearLimits.m_springDampingLimited[i];
limot.m_equilibriumPoint = m_linearLimits.m_equilibriumPoint[i];
limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT2);
limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP2) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
@@ -785,12 +787,12 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
//limit stiffness (the spring should not be sampled faster that the quarter of its angular frequency)
if( 0.25 < angularfreq * dt)
if(limot->m_springStiffnessLimited && 0.25 < angularfreq * dt)
{
ks = BT_ONE / dt / dt / btScalar(16.0) / m;
ks = BT_ONE / dt / dt / btScalar(16.0) * m;
}
//avoid overdamping
if(kd * dt > m)
//avoid damping that would blow up the spring
if(limot->m_springDampingLimited && kd * dt > m)
{
kd = m / dt;
}
@@ -1023,22 +1025,28 @@ void btGeneric6DofSpring2Constraint::enableSpring(int index, bool onOff)
m_angularLimits[index - 3] .m_enableSpring = onOff;
}
void btGeneric6DofSpring2Constraint::setStiffness(int index, btScalar stiffness)
void btGeneric6DofSpring2Constraint::setStiffness(int index, btScalar stiffness, bool limitIfNeeded)
{
btAssert((index >= 0) && (index < 6));
if (index<3)
if (index<3) {
m_linearLimits.m_springStiffness[index] = stiffness;
else
m_angularLimits[index - 3] .m_springStiffness = stiffness;
m_linearLimits.m_springStiffnessLimited[index] = limitIfNeeded;
} else {
m_angularLimits[index - 3].m_springStiffness = stiffness;
m_angularLimits[index - 3].m_springStiffnessLimited = limitIfNeeded;
}
}
void btGeneric6DofSpring2Constraint::setDamping(int index, btScalar damping)
void btGeneric6DofSpring2Constraint::setDamping(int index, btScalar damping, bool limitIfNeeded)
{
btAssert((index >= 0) && (index < 6));
if (index<3)
if (index<3) {
m_linearLimits.m_springDamping[index] = damping;
else
m_angularLimits[index - 3] .m_springDamping = damping;
m_linearLimits.m_springDampingLimited[index] = limitIfNeeded;
} else {
m_angularLimits[index - 3].m_springDamping = damping;
m_angularLimits[index - 3].m_springDampingLimited = limitIfNeeded;
}
}
void btGeneric6DofSpring2Constraint::setEquilibriumPoint()

144
src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h
View File

@@ -86,7 +86,9 @@ public:
btScalar m_servoTarget;
bool m_enableSpring;
btScalar m_springStiffness;
bool m_springStiffnessLimited;
btScalar m_springDamping;
bool m_springDampingLimited;
btScalar m_equilibriumPoint;
btScalar m_currentLimitError;
@@ -96,22 +98,24 @@ public:
btRotationalLimitMotor2()
{
m_loLimit = 1.0f;
m_hiLimit = -1.0f;
m_bounce = 0.0f;
m_stopERP = 0.2f;
m_stopCFM = 0.f;
m_motorERP = 0.9f;
m_motorCFM = 0.f;
m_enableMotor = false;
m_targetVelocity = 0;
m_maxMotorForce = 0.1f;
m_servoMotor = false;
m_servoTarget = 0;
m_enableSpring = false;
m_springStiffness = 0;
m_springDamping = 0;
m_equilibriumPoint = 0;
m_loLimit = 1.0f;
m_hiLimit = -1.0f;
m_bounce = 0.0f;
m_stopERP = 0.2f;
m_stopCFM = 0.f;
m_motorERP = 0.9f;
m_motorCFM = 0.f;
m_enableMotor = false;
m_targetVelocity = 0;
m_maxMotorForce = 0.1f;
m_servoMotor = false;
m_servoTarget = 0;
m_enableSpring = false;
m_springStiffness = 0;
m_springStiffnessLimited = false;
m_springDamping = 0;
m_springDampingLimited = false;
m_equilibriumPoint = 0;
m_currentLimitError = 0;
m_currentLimitErrorHi = 0;
@@ -121,22 +125,24 @@ public:
btRotationalLimitMotor2(const btRotationalLimitMotor2 & limot)
{
m_loLimit = limot.m_loLimit;
m_hiLimit = limot.m_hiLimit;
m_bounce = limot.m_bounce;
m_stopERP = limot.m_stopERP;
m_stopCFM = limot.m_stopCFM;
m_motorERP = limot.m_motorERP;
m_motorCFM = limot.m_motorCFM;
m_enableMotor = limot.m_enableMotor;
m_targetVelocity = limot.m_targetVelocity;
m_maxMotorForce = limot.m_maxMotorForce;
m_servoMotor = limot.m_servoMotor;
m_servoTarget = limot.m_servoTarget;
m_enableSpring = limot.m_enableSpring;
m_springStiffness = limot.m_springStiffness;
m_springDamping = limot.m_springDamping;
m_equilibriumPoint = limot.m_equilibriumPoint;
m_loLimit = limot.m_loLimit;
m_hiLimit = limot.m_hiLimit;
m_bounce = limot.m_bounce;
m_stopERP = limot.m_stopERP;
m_stopCFM = limot.m_stopCFM;
m_motorERP = limot.m_motorERP;
m_motorCFM = limot.m_motorCFM;
m_enableMotor = limot.m_enableMotor;
m_targetVelocity = limot.m_targetVelocity;
m_maxMotorForce = limot.m_maxMotorForce;
m_servoMotor = limot.m_servoMotor;
m_servoTarget = limot.m_servoTarget;
m_enableSpring = limot.m_enableSpring;
m_springStiffness = limot.m_springStiffness;
m_springStiffnessLimited = limot.m_springStiffnessLimited;
m_springDamping = limot.m_springDamping;
m_springDampingLimited = limot.m_springDampingLimited;
m_equilibriumPoint = limot.m_equilibriumPoint;
m_currentLimitError = limot.m_currentLimitError;
m_currentLimitErrorHi = limot.m_currentLimitErrorHi;
@@ -174,7 +180,9 @@ public:
bool m_enableSpring[3];
btVector3 m_servoTarget;
btVector3 m_springStiffness;
bool m_springStiffnessLimited[3];
btVector3 m_springDamping;
bool m_springDampingLimited[3];
btVector3 m_equilibriumPoint;
btVector3 m_targetVelocity;
btVector3 m_maxMotorForce;
@@ -200,15 +208,17 @@ public:
for(int i=0; i < 3; i++)
{
m_enableMotor[i] = false;
m_servoMotor[i] = false;
m_enableSpring[i] = false;
m_servoTarget[i] = btScalar(0.f);
m_springStiffness[i] = btScalar(0.f);
m_springDamping[i] = btScalar(0.f);
m_equilibriumPoint[i] = btScalar(0.f);
m_targetVelocity[i] = btScalar(0.f);
m_maxMotorForce[i] = btScalar(0.f);
m_enableMotor[i] = false;
m_servoMotor[i] = false;
m_enableSpring[i] = false;
m_servoTarget[i] = btScalar(0.f);
m_springStiffness[i] = btScalar(0.f);
m_springStiffnessLimited[i] = false;
m_springDamping[i] = btScalar(0.f);
m_springDampingLimited[i] = false;
m_equilibriumPoint[i] = btScalar(0.f);
m_targetVelocity[i] = btScalar(0.f);
m_maxMotorForce[i] = btScalar(0.f);
m_currentLimit[i] = 0;
}
@@ -230,15 +240,17 @@ public:
for(int i=0; i < 3; i++)
{
m_enableMotor[i] = other.m_enableMotor[i];
m_servoMotor[i] = other.m_servoMotor[i];
m_enableSpring[i] = other.m_enableSpring[i];
m_servoTarget[i] = other.m_servoTarget[i];
m_springStiffness[i] = other.m_springStiffness[i];
m_springDamping[i] = other.m_springDamping[i];
m_equilibriumPoint[i] = other.m_equilibriumPoint[i];
m_targetVelocity[i] = other.m_targetVelocity[i];
m_maxMotorForce[i] = other.m_maxMotorForce[i];
m_enableMotor[i] = other.m_enableMotor[i];
m_servoMotor[i] = other.m_servoMotor[i];
m_enableSpring[i] = other.m_enableSpring[i];
m_servoTarget[i] = other.m_servoTarget[i];
m_springStiffness[i] = other.m_springStiffness[i];
m_springStiffnessLimited[i] = other.m_springStiffnessLimited[i];
m_springDamping[i] = other.m_springDamping[i];
m_springDampingLimited[i] = other.m_springDampingLimited[i];
m_equilibriumPoint[i] = other.m_equilibriumPoint[i];
m_targetVelocity[i] = other.m_targetVelocity[i];
m_maxMotorForce[i] = other.m_maxMotorForce[i];
m_currentLimit[i] = other.m_currentLimit[i];
}
@@ -467,8 +479,8 @@ public:
void setMaxMotorForce(int index, btScalar force);
void enableSpring(int index, bool onOff);
void setStiffness(int index, btScalar stiffness);
void setDamping(int index, btScalar damping);
void setStiffness(int index, btScalar stiffness, bool limitIfNeeded = true); // if limitIfNeeded is true the system will automatically limit the stiffness in necessary situations where otherwise the spring would move unrealistically too widely
void setDamping(int index, btScalar damping, bool limitIfNeeded = true); // if limitIfNeeded is true the system will automatically limit the damping in necessary situations where otherwise the spring would blow up
void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF
void setEquilibriumPoint(int index); // set the current constraint position/orientation as an equilibrium point for given DOF
void setEquilibriumPoint(int index, btScalar val);
@@ -502,6 +514,8 @@ struct btGeneric6DofSpring2ConstraintData
char m_linearEnableMotor[4];
char m_linearServoMotor[4];
char m_linearEnableSpring[4];
char m_linearSpringStiffnessLimited[4];
char m_linearSpringDampingLimited[4];
char m_padding1[4];
btVector3FloatData m_angularUpperLimit;
@@ -520,10 +534,10 @@ struct btGeneric6DofSpring2ConstraintData
char m_angularEnableMotor[4];
char m_angularServoMotor[4];
char m_angularEnableSpring[4];
char m_padding2[4];
char m_angularSpringStiffnessLimited[4];
char m_angularSpringDampingLimited[4];
int m_rotateOrder;
char m_padding3[4];
};
struct btGeneric6DofSpring2ConstraintDoubleData2
@@ -548,6 +562,8 @@ struct btGeneric6DofSpring2ConstraintDoubleData2
char m_linearEnableMotor[4];
char m_linearServoMotor[4];
char m_linearEnableSpring[4];
char m_linearSpringStiffnessLimited[4];
char m_linearSpringDampingLimited[4];
char m_padding1[4];
btVector3DoubleData m_angularUpperLimit;
@@ -566,10 +582,10 @@ struct btGeneric6DofSpring2ConstraintDoubleData2
char m_angularEnableMotor[4];
char m_angularServoMotor[4];
char m_angularEnableSpring[4];
char m_padding2[4];
char m_angularSpringStiffnessLimited[4];
char m_angularSpringDampingLimited[4];
int m_rotateOrder;
char m_padding3[4];
};
SIMD_FORCE_INLINE int btGeneric6DofSpring2Constraint::calculateSerializeBufferSize() const
@@ -617,9 +633,11 @@ SIMD_FORCE_INLINE const char* btGeneric6DofSpring2Constraint::serialize(void* da
dof->m_angularEquilibriumPoint.m_floats[3] = 0;
for (i=0;i<4;i++)
{
dof->m_angularEnableMotor[i] = i < 3 ? ( m_angularLimits[i].m_enableMotor ? 1 : 0 ) : 0;
dof->m_angularServoMotor[i] = i < 3 ? ( m_angularLimits[i].m_servoMotor ? 1 : 0 ) : 0;
dof->m_angularEnableSpring[i] = i < 3 ? ( m_angularLimits[i].m_enableSpring ? 1 : 0 ) : 0;
dof->m_angularEnableMotor[i] = i < 3 ? ( m_angularLimits[i].m_enableMotor ? 1 : 0 ) : 0;
dof->m_angularServoMotor[i] = i < 3 ? ( m_angularLimits[i].m_servoMotor ? 1 : 0 ) : 0;
dof->m_angularEnableSpring[i] = i < 3 ? ( m_angularLimits[i].m_enableSpring ? 1 : 0 ) : 0;
dof->m_angularSpringStiffnessLimited[i] = i < 3 ? ( m_angularLimits[i].m_springStiffnessLimited ? 1 : 0 ) : 0;
dof->m_angularSpringDampingLimited[i] = i < 3 ? ( m_angularLimits[i].m_springDampingLimited ? 1 : 0 ) : 0;
}
m_linearLimits.m_lowerLimit.serialize( dof->m_linearLowerLimit );
@@ -637,9 +655,11 @@ SIMD_FORCE_INLINE const char* btGeneric6DofSpring2Constraint::serialize(void* da
m_linearLimits.m_equilibriumPoint.serialize( dof->m_linearEquilibriumPoint );
for (i=0;i<4;i++)
{
dof->m_linearEnableMotor[i] = i < 3 ? ( m_linearLimits.m_enableMotor[i] ? 1 : 0 ) : 0;
dof->m_linearServoMotor[i] = i < 3 ? ( m_linearLimits.m_servoMotor[i] ? 1 : 0 ) : 0;
dof->m_linearEnableSpring[i] = i < 3 ? ( m_linearLimits.m_enableSpring[i] ? 1 : 0 ) : 0;
dof->m_linearEnableMotor[i] = i < 3 ? ( m_linearLimits.m_enableMotor[i] ? 1 : 0 ) : 0;
dof->m_linearServoMotor[i] = i < 3 ? ( m_linearLimits.m_servoMotor[i] ? 1 : 0 ) : 0;
dof->m_linearEnableSpring[i] = i < 3 ? ( m_linearLimits.m_enableSpring[i] ? 1 : 0 ) : 0;
dof->m_linearSpringStiffnessLimited[i] = i < 3 ? ( m_linearLimits.m_springStiffnessLimited[i] ? 1 : 0 ) : 0;
dof->m_linearSpringDampingLimited[i] = i < 3 ? ( m_linearLimits.m_springDampingLimited[i] ? 1 : 0 ) : 0;
}
dof->m_rotateOrder = m_rotateOrder;