diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index 03a851219..8067de944 100644 --- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -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; +} diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h index 24827a6ba..1f7ff18d6 100644 --- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h @@ -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; diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index 9c79ade2f..653975217 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -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; +} diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h index bf0f3d731..0769f83ff 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -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) diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index aa42d760f..04ec9a673 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -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; +} + + diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index a43ab654d..8058b3ed8 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -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) diff --git a/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp index 0c58b907d..35aa58714 100644 --- a/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp @@ -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; +} + diff --git a/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h b/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h index baf94fc55..bf2d24539 100644 --- a/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h @@ -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) diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 7e25451b0..e8b048a61 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -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 = ¤tConstraintRow->m_rhs; + currentConstraintRow->m_cfm = infoGlobal.m_globalCfm; info2.cfm = ¤tConstraintRow->m_cfm; info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit; info2.m_upperLimit = ¤tConstraintRow->m_upperLimit; diff --git a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp index 30b89b705..91f226eba 100755 --- a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp @@ -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; +} + + + diff --git a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h index 3519b1673..6a6f1ed2c 100755 --- a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h @@ -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) diff --git a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp index 1fa9723ab..ea72a640d 100644 --- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp @@ -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; diff --git a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h index 43227e056..69b289d27 100644 --- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -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;