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

Provide easier access to CFM and ERP (and Stop ERP/Stop CFM) for constraints in a similar way to Open Dynamics Engine

Browse Source 
virtual	void	btTypedConstraint::setParam(int num, btScalar value, int axis = -1) = 0;
virtual	btScalar btTypedConstraint::getParam(int num, int axis = -1) const = 0;
	
Parameter can be BT_CONSTRAINT_ERP,BT_CONSTRAINT_STOP_ERP,BT_CONSTRAINT_CFM,BT_CONSTRAINT_STOP_CFM
Axis is 0 .. 5, first 3 for linear degrees of freedom, last 3 for angular. If no axis is specified it will take the 'default' degree of freedom. For a btHingeConstraint this would be the hinge axis (5)
This commit is contained in:
erwin.coumans
2010-02-03 22:16:09 +00:00
parent d2a55dee59
commit d58081ce37
13 changed files with 790 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -73,6 +73,10 @@ void btConeTwistConstraint::init()
setLimit(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
m_damping = btScalar(0.01);
m_fixThresh = CONETWIST_DEF_FIX_THRESH;
m_flags = 0;
m_linCFM = btScalar(0.f);
m_linERP = btScalar(0.7f);
m_angCFM = btScalar(0.f);
}
@@ -145,13 +149,18 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt
a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
}
// set right hand side
btScalar k = info->fps * info->erp;
btScalar linERP = (m_flags & BT_CONETWIST_FLAGS_LIN_ERP) ? m_linERP : info->erp;
btScalar k = info->fps * linERP;
int j;
for (j=0; j<3; j++)
{
info->m_constraintError[j*info->rowskip] = k * (a2[j] + transB.getOrigin()[j] - a1[j] - transA.getOrigin()[j]);
info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY;
info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY;
if(m_flags & BT_CONETWIST_FLAGS_LIN_CFM)
{
info->cfm[j*info->rowskip] = m_linCFM;
}
}
int row = 3;
int srow = row * info->rowskip;
@@ -200,7 +209,10 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt
btScalar k = info->fps * m_biasFactor;
info->m_constraintError[srow] = k * m_swingCorrection;
info->cfm[srow] = 0.0f;
if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
{
info->cfm[srow] = m_angCFM;
}
// m_swingCorrection is always positive or 0
info->m_lowerLimit[srow] = 0;
info->m_upperLimit[srow] = SIMD_INFINITY;
@@ -220,7 +232,10 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt
J2[srow+2] = -ax1[2];
btScalar k = info->fps * m_biasFactor;
info->m_constraintError[srow] = k * m_twistCorrection;
info->cfm[srow] = 0.0f;
if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
{
info->cfm[srow] = m_angCFM;
}
if(m_twistSpan > 0.0f)
{
@@ -1021,6 +1036,87 @@ void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &
}
}
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
void btConeTwistConstraint::setParam(int num, btScalar value, int axis)
{
switch(num)
{
case BT_CONSTRAINT_ERP :
case BT_CONSTRAINT_STOP_ERP :
if((axis >= 0) && (axis < 3))
{
m_linERP = value;
m_flags |= BT_CONETWIST_FLAGS_LIN_ERP;
}
else
{
m_biasFactor = value;
}
break;
case BT_CONSTRAINT_CFM :
case BT_CONSTRAINT_STOP_CFM :
if((axis >= 0) && (axis < 3))
{
m_linCFM = value;
m_flags |= BT_CONETWIST_FLAGS_LIN_CFM;
}
else
{
m_angCFM = value;
m_flags |= BT_CONETWIST_FLAGS_ANG_CFM;
}
break;
default:
btAssertConstrParams(0);
break;
}
}
///return the local value of parameter
btScalar btConeTwistConstraint::getParam(int num, int axis) const
{
btScalar retVal = 0;
switch(num)
{
case BT_CONSTRAINT_ERP :
case BT_CONSTRAINT_STOP_ERP :
if((axis >= 0) && (axis < 3))
{
btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_ERP);
retVal = m_linERP;
}
else if((axis >= 3) && (axis < 6))
{
retVal = m_biasFactor;
}
else
{
btAssertConstrParams(0);
}
break;
case BT_CONSTRAINT_CFM :
case BT_CONSTRAINT_STOP_CFM :
if((axis >= 0) && (axis < 3))
{
btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_CFM);
retVal = m_linCFM;
}
else if((axis >= 3) && (axis < 6))
{
btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_ANG_CFM);
retVal = m_angCFM;
}
else
{
btAssertConstrParams(0);
}
break;
default :
btAssertConstrParams(0);
}
return retVal;
}

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

@@ -42,6 +42,12 @@ and swing 1 and 2 are along the z and y axes respectively.
class btRigidBody;
enum btConeTwistFlags
{
BT_CONETWIST_FLAGS_LIN_CFM = 1,
BT_CONETWIST_FLAGS_LIN_ERP = 2,
BT_CONETWIST_FLAGS_ANG_CFM = 4
};
///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc)
class btConeTwistConstraint : public btTypedConstraint
@@ -99,6 +105,11 @@ public:
btScalar m_maxMotorImpulse;
btVector3 m_accMotorImpulse;
// parameters
int m_flags;
btScalar m_linCFM;
btScalar m_linERP;
btScalar m_angCFM;
protected:
@@ -256,6 +267,11 @@ public:
btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const;
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
virtual void setParam(int num, btScalar value, int axis = -1);
///return the local value of parameter
virtual btScalar getParam(int num, int axis = -1) const;
virtual int calculateSerializeBufferSize() const;

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

@@ -35,6 +35,7 @@ btGeneric6DofConstraint::btGeneric6DofConstraint()
:btTypedConstraint(D6_CONSTRAINT_TYPE),
m_useLinearReferenceFrameA(true),
m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
m_flags(0),
m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
{
}
@@ -47,6 +48,7 @@ btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody&
, m_frameInB(frameInB),
m_useLinearReferenceFrameA(useLinearReferenceFrameA),
m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
m_flags(0),
m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
{
calculateTransforms();
@@ -58,6 +60,7 @@ btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbB, const btTrans
: btTypedConstraint(D6_CONSTRAINT_TYPE, s_fixed, rbB),
m_frameInB(frameInB),
m_useLinearReferenceFrameA(useLinearReferenceFrameB),
m_flags(0),
m_useSolveConstraintObsolete(false)
{
///not providing rigidbody A means implicitly using worldspace for body A
@@ -161,7 +164,7 @@ btScalar btRotationalLimitMotor::solveAngularLimits(
//current error correction
if (m_currentLimit!=0)
{
target_velocity = -m_ERP*m_currentLimitError/(timeStep);
target_velocity = -m_stopERP*m_currentLimitError/(timeStep);
maxMotorForce = m_maxLimitForce;
}
@@ -614,7 +617,6 @@ int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, c
limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
limot.m_damping = m_linearLimits.m_damping;
limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
limot.m_ERP = m_linearLimits.m_restitution;
limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
@@ -622,6 +624,10 @@ int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, c
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_SHIFT);
limot.m_normalCFM = (flags & BT_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
if(m_useOffsetForConstraintFrame)
{
int indx1 = (i + 1) % 3;
@@ -654,6 +660,19 @@ int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_o
if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
{
btVector3 axis = d6constraint->getAxis(i);
int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT);
if(!(flags & BT_6DOF_FLAGS_CFM_NORM))
{
m_angularLimits[i].m_normalCFM = info->cfm[0];
}
if(!(flags & BT_6DOF_FLAGS_CFM_STOP))
{
m_angularLimits[i].m_stopCFM = info->cfm[0];
}
if(!(flags & BT_6DOF_FLAGS_ERP_STOP))
{
m_angularLimits[i].m_stopERP = info->erp;
}
row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),
transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
}
@@ -828,7 +847,7 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
info->m_constraintError[srow] = btScalar(0.f);
if (powered)
{
info->cfm[srow] = 0.0f;
info->cfm[srow] = limot->m_normalCFM;
if(!limit)
{
btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
@@ -845,7 +864,7 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
}
if(limit)
{
btScalar k = info->fps * limot->m_ERP;
btScalar k = info->fps * limot->m_stopERP;
if(!rotational)
{
info->m_constraintError[srow] += k * limot->m_currentLimitError;
@@ -854,7 +873,7 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
{
info->m_constraintError[srow] += -k * limot->m_currentLimitError;
}
info->cfm[srow] = 0.0f;
info->cfm[srow] = limot->m_stopCFM;
if (limot->m_loLimit == limot->m_hiLimit)
{ // limited low and high simultaneously
info->m_lowerLimit[srow] = -SIMD_INFINITY;
@@ -978,7 +997,7 @@ int btGeneric6DofConstraint::get_limit_motor_info2UsingFrameOffset( btRotational
info->m_constraintError[srow] = btScalar(0.f);
if (powered)
{
info->cfm[srow] = 0.0f;
info->cfm[srow] = limot->m_normalCFM;
if(!limit)
{
btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
@@ -987,7 +1006,7 @@ int btGeneric6DofConstraint::get_limit_motor_info2UsingFrameOffset( btRotational
limot->m_loLimit,
limot->m_hiLimit,
tag_vel,
info->fps * info->erp);
info->fps * limot->m_stopERP);
info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
info->m_upperLimit[srow] = limot->m_maxMotorForce;
@@ -995,7 +1014,7 @@ int btGeneric6DofConstraint::get_limit_motor_info2UsingFrameOffset( btRotational
}
if(limit)
{
btScalar k = info->fps * limot->m_ERP;
btScalar k = info->fps * limot->m_stopERP;
if(!rotational)
{
info->m_constraintError[srow] += k * limot->m_currentLimitError;
@@ -1004,7 +1023,7 @@ int btGeneric6DofConstraint::get_limit_motor_info2UsingFrameOffset( btRotational
{
info->m_constraintError[srow] += -k * limot->m_currentLimitError;
}
info->cfm[srow] = 0.0f;
info->cfm[srow] = limot->m_stopCFM;
if (limot->m_loLimit == limot->m_hiLimit)
{ // limited low and high simultaneously
info->m_lowerLimit[srow] = -SIMD_INFINITY;
@@ -1069,3 +1088,105 @@ int btGeneric6DofConstraint::get_limit_motor_info2UsingFrameOffset( btRotational
else return 0;
}
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis)
{
if((axis >= 0) && (axis < 3))
{
switch(num)
{
case BT_CONSTRAINT_STOP_ERP :
m_linearLimits.m_stopERP[axis] = value;
m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
break;
case BT_CONSTRAINT_STOP_CFM :
m_linearLimits.m_stopCFM[axis] = value;
m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
break;
case BT_CONSTRAINT_CFM :
m_linearLimits.m_normalCFM[axis] = value;
m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
break;
default :
btAssertConstrParams(0);
}
}
else if((axis >=3) && (axis < 6))
{
switch(num)
{
case BT_CONSTRAINT_STOP_ERP :
m_angularLimits[axis - 3].m_stopERP = value;
m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
break;
case BT_CONSTRAINT_STOP_CFM :
m_angularLimits[axis - 3].m_stopCFM = value;
m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
break;
case BT_CONSTRAINT_CFM :
m_angularLimits[axis - 3].m_normalCFM = value;
m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
break;
default :
btAssertConstrParams(0);
}
}
else
{
btAssertConstrParams(0);
}
}
///return the local value of parameter
btScalar btGeneric6DofConstraint::getParam(int num, int axis) const
{
btScalar retVal = 0;
if((axis >= 0) && (axis < 3))
{
switch(num)
{
case BT_CONSTRAINT_STOP_ERP :
btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
retVal = m_linearLimits.m_stopERP[axis];
break;
case BT_CONSTRAINT_STOP_CFM :
btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
retVal = m_linearLimits.m_stopCFM[axis];
break;
case BT_CONSTRAINT_CFM :
btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
retVal = m_linearLimits.m_normalCFM[axis];
break;
default :
btAssertConstrParams(0);
}
}
else if((axis >=3) && (axis < 6))
{
switch(num)
{
case BT_CONSTRAINT_STOP_ERP :
btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
retVal = m_angularLimits[axis - 3].m_stopERP;
break;
case BT_CONSTRAINT_STOP_CFM :
btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
retVal = m_angularLimits[axis - 3].m_stopCFM;
break;
case BT_CONSTRAINT_CFM :
btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
retVal = m_angularLimits[axis - 3].m_normalCFM;
break;
default :
btAssertConstrParams(0);
}
}
else
{
btAssertConstrParams(0);
}
return retVal;
}

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

@@ -49,7 +49,9 @@ public:
btScalar m_maxLimitForce;//!< max force on limit
btScalar m_damping;//!< Damping.
btScalar m_limitSoftness;//! Relaxation factor
btScalar m_ERP;//!< Error tolerance factor when joint is at limit
btScalar m_normalCFM;//!< Constraint force mixing factor
btScalar m_stopERP;//!< Error tolerance factor when joint is at limit
btScalar m_stopCFM;//!< Constraint force mixing factor when joint is at limit
btScalar m_bounce;//!< restitution factor
bool m_enableMotor;
@@ -71,7 +73,9 @@ public:
m_maxLimitForce = 300.0f;
m_loLimit = 1.0f;
m_hiLimit = -1.0f;
m_ERP = 0.5f;
m_normalCFM = 0.f;
m_stopERP = 0.2f;
m_stopCFM = 0.f;
m_bounce = 0.0f;
m_damping = 1.0f;
m_limitSoftness = 0.5f;
@@ -87,7 +91,9 @@ public:
m_limitSoftness = limot.m_limitSoftness;
m_loLimit = limot.m_loLimit;
m_hiLimit = limot.m_hiLimit;
m_ERP = limot.m_ERP;
m_normalCFM = limot.m_normalCFM;
m_stopERP = limot.m_stopERP;
m_stopCFM = limot.m_stopCFM;
m_bounce = limot.m_bounce;
m_currentLimit = limot.m_currentLimit;
m_currentLimitError = limot.m_currentLimitError;
@@ -134,6 +140,9 @@ public:
btScalar m_limitSoftness;//!< Softness for linear limit
btScalar m_damping;//!< Damping for linear limit
btScalar m_restitution;//! Bounce parameter for linear limit
btVector3 m_normalCFM;//!< Constraint force mixing factor
btVector3 m_stopERP;//!< Error tolerance factor when joint is at limit
btVector3 m_stopCFM;//!< Constraint force mixing factor when joint is at limit
//!@}
bool m_enableMotor[3];
btVector3 m_targetVelocity;//!< target motor velocity
@@ -147,6 +156,9 @@ public:
m_lowerLimit.setValue(0.f,0.f,0.f);
m_upperLimit.setValue(0.f,0.f,0.f);
m_accumulatedImpulse.setValue(0.f,0.f,0.f);
m_normalCFM.setValue(0.f, 0.f, 0.f);
m_stopERP.setValue(0.2f, 0.2f, 0.2f);
m_stopCFM.setValue(0.f, 0.f, 0.f);
m_limitSoftness = 0.7f;
m_damping = btScalar(1.0f);
@@ -168,6 +180,10 @@ public:
m_limitSoftness = other.m_limitSoftness ;
m_damping = other.m_damping;
m_restitution = other.m_restitution;
m_normalCFM = other.m_normalCFM;
m_stopERP = other.m_stopERP;
m_stopCFM = other.m_stopCFM;
for(int i=0; i < 3; i++)
{
m_enableMotor[i] = other.m_enableMotor[i];
@@ -207,6 +223,15 @@ public:
};
enum bt6DofFlags
{
BT_6DOF_FLAGS_CFM_NORM = 1,
BT_6DOF_FLAGS_CFM_STOP = 2,
BT_6DOF_FLAGS_ERP_STOP = 4
};
#define BT_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis
/// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
/*!
btGeneric6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'.
@@ -289,6 +314,8 @@ protected:
bool m_useLinearReferenceFrameA;
bool m_useOffsetForConstraintFrame;
int m_flags;
//!@}
btGeneric6DofConstraint& operator=(btGeneric6DofConstraint& other)
@@ -499,6 +526,12 @@ public:
bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
virtual void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis = -1);
///return the local value of parameter
virtual btScalar btGeneric6DofConstraint::getParam(int num, int axis = -1) const;
virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)

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

@@ -35,6 +35,7 @@ btHingeConstraint::btHingeConstraint()
m_enableAngularMotor(false),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_flags(0),
m_useReferenceFrameA(false)
{
m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
@@ -47,6 +48,7 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
m_angularOnly(false),
m_enableAngularMotor(false),
m_flags(0),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA)
@@ -98,6 +100,7 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_flags(0),
m_useReferenceFrameA(useReferenceFrameA)
{
@@ -142,6 +145,7 @@ m_angularOnly(false),
m_enableAngularMotor(false),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_flags(0),
m_useReferenceFrameA(useReferenceFrameA)
{
//start with free
@@ -162,6 +166,7 @@ m_angularOnly(false),
m_enableAngularMotor(false),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_flags(0),
m_useReferenceFrameA(useReferenceFrameA)
{
///not providing rigidbody B means implicitly using worldspace for body B
@@ -633,19 +638,26 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf
powered = 0;
}
info->m_constraintError[srow] = btScalar(0.0f);
btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
if(powered)
{
info->cfm[srow] = btScalar(0.0);
btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * info->erp);
if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
{
info->cfm[srow] = m_normalCFM;
}
btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
info->m_lowerLimit[srow] = - m_maxMotorImpulse;
info->m_upperLimit[srow] = m_maxMotorImpulse;
}
if(limit)
{
k = info->fps * info->erp;
k = info->fps * currERP;
info->m_constraintError[srow] += k * limit_err;
info->cfm[srow] = btScalar(0.0);
if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
{
info->cfm[srow] = m_stopCFM;
}
if(lostop == histop)
{
// limited low and high simultaneously
@@ -1010,19 +1022,26 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
powered = 0;
}
info->m_constraintError[srow] = btScalar(0.0f);
btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
if(powered)
{
info->cfm[srow] = btScalar(0.0);
btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * info->erp);
if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
{
info->cfm[srow] = m_normalCFM;
}
btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
info->m_lowerLimit[srow] = - m_maxMotorImpulse;
info->m_upperLimit[srow] = m_maxMotorImpulse;
}
if(limit)
{
k = info->fps * info->erp;
k = info->fps * currERP;
info->m_constraintError[srow] += k * limit_err;
info->cfm[srow] = btScalar(0.0);
if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
{
info->cfm[srow] = m_stopCFM;
}
if(lostop == histop)
{
// limited low and high simultaneously
@@ -1075,3 +1094,66 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
} // if angular limit or powered
}
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
void btHingeConstraint::setParam(int num, btScalar value, int axis)
{
if((axis == -1) || (axis == 5))
{
switch(num)
{
case BT_CONSTRAINT_STOP_ERP :
m_stopERP = value;
m_flags |= BT_HINGE_FLAGS_ERP_STOP;
break;
case BT_CONSTRAINT_STOP_CFM :
m_stopCFM = value;
m_flags |= BT_HINGE_FLAGS_CFM_STOP;
break;
case BT_CONSTRAINT_CFM :
m_normalCFM = value;
m_flags |= BT_HINGE_FLAGS_CFM_NORM;
break;
default :
btAssertConstrParams(0);
}
}
else
{
btAssertConstrParams(0);
}
}
///return the local value of parameter
btScalar btHingeConstraint::getParam(int num, int axis) const
{
btScalar retVal = 0;
if((axis == -1) || (axis == 5))
{
switch(num)
{
case BT_CONSTRAINT_STOP_ERP :
btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_STOP);
retVal = m_stopERP;
break;
case BT_CONSTRAINT_STOP_CFM :
btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_STOP);
retVal = m_stopCFM;
break;
case BT_CONSTRAINT_CFM :
btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM);
retVal = m_normalCFM;
break;
default :
btAssertConstrParams(0);
}
}
else
{
btAssertConstrParams(0);
}
return retVal;
}

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

@@ -33,6 +33,14 @@ class btRigidBody;
#endif //BT_USE_DOUBLE_PRECISION
enum btHingeFlags
{
BT_HINGE_FLAGS_CFM_STOP = 1,
BT_HINGE_FLAGS_ERP_STOP = 2,
BT_HINGE_FLAGS_CFM_NORM = 4
};
/// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
/// axis defines the orientation of the hinge axis
ATTRIBUTE_ALIGNED16(class) btHingeConstraint : public btTypedConstraint
@@ -74,6 +82,11 @@ public:
btScalar m_accMotorImpulse;
int m_flags;
btScalar m_normalCFM;
btScalar m_stopCFM;
btScalar m_stopERP;
public:
@@ -232,6 +245,13 @@ public:
bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
virtual void setParam(int num, btScalar value, int axis = -1);
///return the local value of parameter
virtual btScalar getParam(int num, int axis = -1) const;
virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)

73
src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
View File

@@ -22,12 +22,14 @@ subject to the following restrictions:
btPoint2PointConstraint::btPoint2PointConstraint()
:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE),
m_flags(0),
m_useSolveConstraintObsolete(false)
{
}
btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB)
:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
m_flags(0),
m_useSolveConstraintObsolete(false)
{
@@ -36,6 +38,7 @@ m_useSolveConstraintObsolete(false)
btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
m_flags(0),
m_useSolveConstraintObsolete(false)
{
@@ -136,14 +139,21 @@ void btPoint2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const
// set right hand side
btScalar k = info->fps * info->erp;
btScalar currERP = (m_flags & BT_P2P_FLAGS_ERP) ? m_erp : info->erp;
btScalar k = info->fps * currERP;
int j;
for (j=0; j<3; j++)
{
info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
}
if(m_flags & BT_P2P_FLAGS_CFM)
{
for (j=0; j<3; j++)
{
info->cfm[j*info->rowskip] = m_cfm;
}
}
btScalar impulseClamp = m_setting.m_impulseClamp;//
for (j=0; j<3; j++)
@@ -240,3 +250,60 @@ void btPoint2PointConstraint::updateRHS(btScalar timeStep)
}
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
void btPoint2PointConstraint::setParam(int num, btScalar value, int axis)
{
if(axis != -1)
{
btAssertConstrParams(0);
}
else
{
switch(num)
{
case BT_CONSTRAINT_ERP :
case BT_CONSTRAINT_STOP_ERP :
m_erp = value;
m_flags |= BT_P2P_FLAGS_ERP;
break;
case BT_CONSTRAINT_CFM :
case BT_CONSTRAINT_STOP_CFM :
m_cfm = value;
m_flags |= BT_P2P_FLAGS_CFM;
break;
default:
btAssertConstrParams(0);
}
}
}
///return the local value of parameter
btScalar btPoint2PointConstraint::getParam(int num, int axis) const
{
btScalar retVal;
if(axis != -1)
{
btAssertConstrParams(0);
}
else
{
switch(num)
{
case BT_CONSTRAINT_ERP :
case BT_CONSTRAINT_STOP_ERP :
btAssertConstrParams(m_flags & BT_P2P_FLAGS_ERP);
retVal = m_erp;
break;
case BT_CONSTRAINT_CFM :
case BT_CONSTRAINT_STOP_CFM :
btAssertConstrParams(m_flags & BT_P2P_FLAGS_CFM);
retVal = m_cfm;
break;
default:
btAssertConstrParams(0);
}
}
return retVal;
}

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

@@ -44,6 +44,12 @@ struct btConstraintSetting
btScalar m_impulseClamp;
};
enum btPoint2PointFlags
{
BT_P2P_FLAGS_ERP = 1,
BT_P2P_FLAGS_CFM = 2
};
/// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
ATTRIBUTE_ALIGNED16(class) btPoint2PointConstraint : public btTypedConstraint
{
@@ -55,7 +61,9 @@ public:
btVector3 m_pivotInA;
btVector3 m_pivotInB;
int m_flags;
btScalar m_erp;
btScalar m_cfm;
public:
@@ -104,6 +112,12 @@ public:
return m_pivotInB;
}
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
virtual void setParam(int num, btScalar value, int axis = -1);
///return the local value of parameter
virtual btScalar getParam(int num, int axis = -1) const;
virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)

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

@@ -809,6 +809,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
///the size of btSolverConstraint needs be a multiple of btScalar
btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
info2.m_constraintError = &currentConstraintRow->m_rhs;
currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
info2.cfm = &currentConstraintRow->m_cfm;
info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
info2.m_upperLimit = &currentConstraintRow->m_upperLimit;

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

@@ -36,21 +36,27 @@ void btSliderConstraint::initParams()
m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
m_dampingDirLin = btScalar(0.);
m_cfmDirLin = SLIDER_CONSTRAINT_DEF_CFM;
m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
m_dampingDirAng = btScalar(0.);
m_cfmDirAng = SLIDER_CONSTRAINT_DEF_CFM;
m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING;
m_cfmOrthoLin = SLIDER_CONSTRAINT_DEF_CFM;
m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING;
m_cfmOrthoAng = SLIDER_CONSTRAINT_DEF_CFM;
m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING;
m_cfmLimLin = SLIDER_CONSTRAINT_DEF_CFM;
m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING;
m_cfmLimAng = SLIDER_CONSTRAINT_DEF_CFM;
m_poweredLinMotor = false;
m_targetLinMotorVelocity = btScalar(0.);
@@ -63,6 +69,7 @@ void btSliderConstraint::initParams()
m_accumulatedAngMotorImpulse = btScalar(0.0);
m_useLinearReferenceFrameA = USE_OFFSET_FOR_CONSTANT_FRAME;
m_flags = 0;
calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
}
@@ -281,11 +288,20 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
// 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.
btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
// btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
btScalar currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTANG) ? m_softnessOrthoAng : m_softnessOrthoAng * info->erp;
btScalar k = info->fps * currERP;
btVector3 ax2 = trB.getBasis().getColumn(0);
btVector3 u = ax1.cross(ax2);
info->m_constraintError[0] = k * u.dot(p);
info->m_constraintError[s] = k * u.dot(q);
if(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG)
{
info->cfm[0] = m_cfmOrthoAng;
info->cfm[s] = m_cfmOrthoAng;
}
// pull out pos and R for both bodies. also get the connection
// vector c = pos2-pos1.
// next two rows. we want: vel2 = vel1 + w1 x c ... but this would
@@ -325,9 +341,18 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
// point (in body 2's frame) with the center of body 1.
btVector3 ofs; // offset point in global coordinates
ofs = trB.getOrigin() - trA.getOrigin();
k = info->fps * info->erp * getSoftnessOrthoLin();
// k = info->fps * info->erp * getSoftnessOrthoLin();
currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN) ? m_softnessOrthoLin : m_softnessOrthoLin * info->erp;
k = info->fps * currERP;
info->m_constraintError[s2] = k * p.dot(ofs);
info->m_constraintError[s3] = k * q.dot(ofs);
if(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN)
{
info->cfm[s2] = m_cfmOrthoLin;
info->cfm[s3] = m_cfmOrthoLin;
}
int nrow = 3; // last filled row
int srow;
// check linear limits linear
@@ -378,11 +403,15 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
info->m_constraintError[srow] = 0.;
info->m_lowerLimit[srow] = 0.;
info->m_upperLimit[srow] = 0.;
currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN) ? m_softnessLimLin : info->erp;
if(powered)
{
info->cfm[nrow] = btScalar(0.0);
if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN)
{
info->cfm[nrow] = m_cfmDirLin;
}
btScalar tag_vel = getTargetLinMotorVelocity();
btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * info->erp);
btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * currERP);
// info->m_constraintError[srow] += mot_fact * getTargetLinMotorVelocity();
info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity();
info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
@@ -390,9 +419,12 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
}
if(limit)
{
k = info->fps * info->erp;
k = info->fps * currERP;
info->m_constraintError[srow] += k * limit_err;
info->cfm[srow] = btScalar(0.0); // stop_cfm;
if(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN)
{
info->cfm[srow] = m_cfmLimLin;
}
if(lostop == histop)
{ // limited low and high simultaneously
info->m_lowerLimit[srow] = -SIMD_INFINITY;
@@ -475,19 +507,26 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
{ // the joint motor is ineffective
powered = 0;
}
currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMANG) ? m_softnessLimAng : info->erp;
if(powered)
{
info->cfm[srow] = btScalar(0.0);
btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * info->erp);
if(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG)
{
info->cfm[nrow] = m_cfmDirAng;
}
btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * currERP);
info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps;
}
if(limit)
{
k = info->fps * info->erp;
k = info->fps * currERP;
info->m_constraintError[srow] += k * limit_err;
info->cfm[srow] = btScalar(0.0); // stop_cfm;
if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG)
{
info->cfm[nrow] = m_cfmLimAng;
}
if(lostop == histop)
{
// limited low and high simultaneously
@@ -930,10 +969,18 @@ void btSliderConstraint::getInfo2NonVirtualUsingFrameOffset(btConstraintInfo2* i
// 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.
btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
// btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
btScalar currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTANG) ? m_softnessOrthoAng : m_softnessOrthoAng * info->erp;
btScalar k = info->fps * currERP;
btVector3 u = ax1A.cross(ax1B);
info->m_constraintError[0] = k * u.dot(p);
info->m_constraintError[s] = k * u.dot(q);
if(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG)
{
info->cfm[0] = m_cfmOrthoAng;
info->cfm[s] = m_cfmOrthoAng;
}
int nrow = 1; // last filled row
int srow;
@@ -1000,11 +1047,22 @@ void btSliderConstraint::getInfo2NonVirtualUsingFrameOffset(btConstraintInfo2* i
for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
// compute two elements of right hand side
k = info->fps * info->erp * getSoftnessOrthoLin();
// k = info->fps * info->erp * getSoftnessOrthoLin();
currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN) ? m_softnessOrthoLin : m_softnessOrthoLin * info->erp;
k = info->fps * currERP;
btScalar rhs = k * p.dot(ofs);
info->m_constraintError[s2] = rhs;
rhs = k * q.dot(ofs);
info->m_constraintError[s3] = rhs;
if(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN)
{
info->cfm[s2] = m_cfmOrthoLin;
info->cfm[s3] = m_cfmOrthoLin;
}
// check linear limits
limit_err = btScalar(0.0);
limit = 0;
@@ -1055,20 +1113,27 @@ void btSliderConstraint::getInfo2NonVirtualUsingFrameOffset(btConstraintInfo2* i
info->m_constraintError[srow] = 0.;
info->m_lowerLimit[srow] = 0.;
info->m_upperLimit[srow] = 0.;
currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN) ? m_softnessLimLin : info->erp;
if(powered)
{
info->cfm[nrow] = btScalar(0.0);
if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN)
{
info->cfm[nrow] = m_cfmDirLin;
}
btScalar tag_vel = getTargetLinMotorVelocity();
btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * info->erp);
btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * currERP);
info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity();
info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps;
}
if(limit)
{
k = info->fps * info->erp;
k = info->fps * currERP;
info->m_constraintError[srow] += k * limit_err;
info->cfm[srow] = btScalar(0.0); // stop_cfm;
if(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN)
{
info->cfm[srow] = m_cfmLimLin;
}
if(lostop == histop)
{ // limited low and high simultaneously
info->m_lowerLimit[srow] = -SIMD_INFINITY;
@@ -1151,19 +1216,27 @@ void btSliderConstraint::getInfo2NonVirtualUsingFrameOffset(btConstraintInfo2* i
{ // the joint motor is ineffective
powered = 0;
}
currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMANG) ? m_softnessLimAng : info->erp;
if(powered)
{
if(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG)
{
info->cfm[nrow] = m_cfmDirAng;
}
info->cfm[srow] = btScalar(0.0);
btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * info->erp);
btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * currERP);
info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps;
}
if(limit)
{
k = info->fps * info->erp;
k = info->fps * currERP;
info->m_constraintError[srow] += k * limit_err;
info->cfm[srow] = btScalar(0.0); // stop_cfm;
if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG)
{
info->cfm[nrow] = m_cfmLimAng;
}
if(lostop == histop)
{
// limited low and high simultaneously
@@ -1215,3 +1288,162 @@ void btSliderConstraint::getInfo2NonVirtualUsingFrameOffset(btConstraintInfo2* i
} // if(limit)
} // if angular limit or powered
}
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
void btSliderConstraint::setParam(int num, btScalar value, int axis)
{
switch(num)
{
case BT_CONSTRAINT_STOP_ERP :
if(axis < 1)
{
m_softnessLimLin = value;
m_flags |= BT_SLIDER_FLAGS_ERP_LIMLIN;
}
else if(axis < 3)
{
m_softnessOrthoLin = value;
m_flags |= BT_SLIDER_FLAGS_ERP_ORTLIN;
}
else if(axis == 3)
{
m_softnessLimAng = value;
m_flags |= BT_SLIDER_FLAGS_ERP_LIMANG;
}
else if(axis < 6)
{
m_softnessOrthoAng = value;
m_flags |= BT_SLIDER_FLAGS_ERP_ORTANG;
}
else
{
btAssertConstrParams(0);
}
break;
case BT_CONSTRAINT_CFM :
if(axis < 1)
{
m_cfmDirLin = value;
m_flags |= BT_SLIDER_FLAGS_CFM_DIRLIN;
}
else if(axis == 3)
{
m_cfmDirAng = value;
m_flags |= BT_SLIDER_FLAGS_CFM_DIRANG;
}
else
{
btAssertConstrParams(0);
}
break;
case BT_CONSTRAINT_STOP_CFM :
if(axis < 1)
{
m_cfmLimLin = value;
m_flags |= BT_SLIDER_FLAGS_CFM_LIMLIN;
}
else if(axis < 3)
{
m_cfmOrthoLin = value;
m_flags |= BT_SLIDER_FLAGS_CFM_ORTLIN;
}
else if(axis == 3)
{
m_cfmLimAng = value;
m_flags |= BT_SLIDER_FLAGS_CFM_LIMANG;
}
else if(axis < 6)
{
m_cfmOrthoAng = value;
m_flags |= BT_SLIDER_FLAGS_CFM_ORTANG;
}
else
{
btAssertConstrParams(0);
}
break;
}
}
///return the local value of parameter
btScalar btSliderConstraint::getParam(int num, int axis) const
{
btScalar retVal;
switch(num)
{
case BT_CONSTRAINT_STOP_ERP :
if(axis < 1)
{
btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN);
retVal = m_softnessLimLin;
}
else if(axis < 3)
{
btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN);
retVal = m_softnessOrthoLin;
}
else if(axis == 3)
{
btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMANG);
retVal = m_softnessLimAng;
}
else if(axis < 6)
{
btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTANG);
retVal = m_softnessOrthoAng;
}
else
{
btAssertConstrParams(0);
}
break;
case BT_CONSTRAINT_CFM :
if(axis < 1)
{
btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN);
retVal = m_cfmDirLin;
}
else if(axis == 3)
{
btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG);
retVal = m_cfmDirAng;
}
else
{
btAssertConstrParams(0);
}
break;
case BT_CONSTRAINT_STOP_CFM :
if(axis < 1)
{
btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN);
retVal = m_cfmLimLin;
}
else if(axis < 3)
{
btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN);
retVal = m_cfmOrthoLin;
}
else if(axis == 3)
{
btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG);
retVal = m_cfmLimAng;
}
else if(axis < 6)
{
btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG);
retVal = m_cfmOrthoAng;
}
else
{
btAssertConstrParams(0);
}
break;
}
return retVal;
}

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

@@ -40,8 +40,25 @@ class btRigidBody;
#define SLIDER_CONSTRAINT_DEF_SOFTNESS (btScalar(1.0))
#define SLIDER_CONSTRAINT_DEF_DAMPING (btScalar(1.0))
#define SLIDER_CONSTRAINT_DEF_RESTITUTION (btScalar(0.7))
#define SLIDER_CONSTRAINT_DEF_CFM (btScalar(0.f))
enum btSliderFlags
{
BT_SLIDER_FLAGS_CFM_DIRLIN = (1 << 0),
BT_SLIDER_FLAGS_ERP_DIRLIN = (1 << 1),
BT_SLIDER_FLAGS_CFM_DIRANG = (1 << 2),
BT_SLIDER_FLAGS_ERP_DIRANG = (1 << 3),
BT_SLIDER_FLAGS_CFM_ORTLIN = (1 << 4),
BT_SLIDER_FLAGS_ERP_ORTLIN = (1 << 5),
BT_SLIDER_FLAGS_CFM_ORTANG = (1 << 6),
BT_SLIDER_FLAGS_ERP_ORTANG = (1 << 7),
BT_SLIDER_FLAGS_CFM_LIMLIN = (1 << 8),
BT_SLIDER_FLAGS_ERP_LIMLIN = (1 << 9),
BT_SLIDER_FLAGS_CFM_LIMANG = (1 << 10),
BT_SLIDER_FLAGS_ERP_LIMANG = (1 << 11)
};
class btSliderConstraint : public btTypedConstraint
{
@@ -68,26 +85,39 @@ protected:
btScalar m_softnessDirLin;
btScalar m_restitutionDirLin;
btScalar m_dampingDirLin;
btScalar m_cfmDirLin;
btScalar m_softnessDirAng;
btScalar m_restitutionDirAng;
btScalar m_dampingDirAng;
btScalar m_cfmDirAng;
btScalar m_softnessLimLin;
btScalar m_restitutionLimLin;
btScalar m_dampingLimLin;
btScalar m_cfmLimLin;
btScalar m_softnessLimAng;
btScalar m_restitutionLimAng;
btScalar m_dampingLimAng;
btScalar m_cfmLimAng;
btScalar m_softnessOrthoLin;
btScalar m_restitutionOrthoLin;
btScalar m_dampingOrthoLin;
btScalar m_cfmOrthoLin;
btScalar m_softnessOrthoAng;
btScalar m_restitutionOrthoAng;
btScalar m_dampingOrthoAng;
btScalar m_cfmOrthoAng;
// for interlal use
bool m_solveLinLim;
bool m_solveAngLim;
int m_flags;
btJacobianEntry m_jacLin[3];
btScalar m_jacLinDiagABInv[3];
@@ -231,6 +261,12 @@ public:
bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
virtual void setParam(int num, btScalar value, int axis = -1);
///return the local value of parameter
virtual btScalar getParam(int num, int axis = -1) const;
virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)

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

@@ -121,9 +121,6 @@ const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* seriali
tcd->m_rbA = (btRigidBodyData*)&m_rbA;
tcd->m_rbB = (btRigidBodyData*)&m_rbB;
m_appliedAngularImpulseA.serializeFloat(tcd->m_appliedAngularImpulseA);
m_appliedAngularImpulseB.serializeFloat(tcd->m_appliedAngularImpulseB);
m_appliedLinearImpulse.serializeFloat(tcd->m_appliedLinearImpulse);
tcd->m_objectType = m_objectType;
tcd->m_needsFeedback = m_needsFeedback;

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

@@ -33,6 +33,22 @@ enum btTypedConstraintType
CONTACT_CONSTRAINT_TYPE
};
enum btConstraintParams
{
BT_CONSTRAINT_ERP=1,
BT_CONSTRAINT_STOP_ERP,
BT_CONSTRAINT_CFM,
BT_CONSTRAINT_STOP_CFM
};
#if 1
#define btAssertConstrParams(_par) btAssert(_par)
#else
#define btAssertConstrParams(_par)
#endif
///TypedConstraint is the baseclass for Bullet constraints and vehicles
class btTypedConstraint : public btTypedObject
{
@@ -53,9 +69,9 @@ protected:
btScalar m_appliedImpulse;
btScalar m_dbgDrawSize;
btVector3 m_appliedLinearImpulse;
btVector3 m_appliedAngularImpulseA;
btVector3 m_appliedAngularImpulseB;
///internal method used by the constraint solver, don't use them directly
btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
public:
@@ -131,8 +147,6 @@ public:
///internal method used by the constraint solver, don't use them directly
virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) = 0;
///internal method used by the constraint solver, don't use them directly
btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
const btRigidBody& getRigidBodyA() const
{
@@ -197,44 +211,6 @@ public:
return m_appliedImpulse;
}
const btVector3& getAppliedLinearImpulse() const
{
btAssert(m_needsFeedback);
return m_appliedLinearImpulse;
}
btVector3& getAppliedLinearImpulse()
{
btAssert(m_needsFeedback);
return m_appliedLinearImpulse;
}
const btVector3& getAppliedAngularImpulseA() const
{
btAssert(m_needsFeedback);
return m_appliedAngularImpulseA;
}
btVector3& getAppliedAngularImpulseA()
{
btAssert(m_needsFeedback);
return m_appliedAngularImpulseA;
}
const btVector3& getAppliedAngularImpulseB() const
{
btAssert(m_needsFeedback);
return m_appliedAngularImpulseB;
}
btVector3& getAppliedAngularImpulseB()
{
btAssert(m_needsFeedback);
return m_appliedAngularImpulseB;
}
btTypedConstraintType getConstraintType () const
{
return btTypedConstraintType(m_objectType);
@@ -248,6 +224,13 @@ public:
{
return m_dbgDrawSize;
}
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
virtual void setParam(int num, btScalar value, int axis = -1) = 0;
///return the local value of parameter
virtual btScalar getParam(int num, int axis = -1) const = 0;
virtual int calculateSerializeBufferSize() const;
@@ -288,10 +271,6 @@ struct btTypedConstraintData
btRigidBodyData *m_rbA;
btRigidBodyData *m_rbB;
btVector3FloatData m_appliedLinearImpulse;
btVector3FloatData m_appliedAngularImpulseA;
btVector3FloatData m_appliedAngularImpulseB;
int m_objectType;
int m_userConstraintType;
int m_userConstraintId;