diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index bc874f75b..144beef1c 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -43,6 +43,9 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useReferenceFrameA(useReferenceFrameA), m_flags(0) +#ifdef _BT_USE_CENTER_LIMIT_ + ,m_limit() +#endif { m_rbAFrame.getOrigin() = pivotInA; @@ -75,6 +78,7 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); +#ifndef _BT_USE_CENTER_LIMIT_ //start with free m_lowerLimit = btScalar(1.0f); m_upperLimit = btScalar(-1.0f); @@ -82,6 +86,7 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; +#endif m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } @@ -93,6 +98,9 @@ m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useReferenceFrameA(useReferenceFrameA), m_flags(0) +#ifdef _BT_USE_CENTER_LIMIT_ +,m_limit() +#endif { // since no frame is given, assume this to be zero angle and just pick rb transform axis @@ -117,6 +125,7 @@ m_flags(0) rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); +#ifndef _BT_USE_CENTER_LIMIT_ //start with free m_lowerLimit = btScalar(1.0f); m_upperLimit = btScalar(-1.0f); @@ -124,6 +133,7 @@ m_flags(0) m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; +#endif m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } @@ -138,7 +148,11 @@ m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useReferenceFrameA(useReferenceFrameA), m_flags(0) +#ifdef _BT_USE_CENTER_LIMIT_ +,m_limit() +#endif { +#ifndef _BT_USE_CENTER_LIMIT_ //start with free m_lowerLimit = btScalar(1.0f); m_upperLimit = btScalar(-1.0f); @@ -146,6 +160,7 @@ m_flags(0) m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; +#endif m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } @@ -159,11 +174,14 @@ m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useReferenceFrameA(useReferenceFrameA), m_flags(0) +#ifdef _BT_USE_CENTER_LIMIT_ +,m_limit() +#endif { ///not providing rigidbody B means implicitly using worldspace for body B m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin()); - +#ifndef _BT_USE_CENTER_LIMIT_ //start with free m_lowerLimit = btScalar(1.0f); m_upperLimit = btScalar(-1.0f); @@ -171,6 +189,7 @@ m_flags(0) m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; +#endif m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); } @@ -449,8 +468,13 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf int limit = 0; if(getSolveLimit()) { - limit_err = m_correction * m_referenceSign; - limit = (limit_err > btScalar(0.0)) ? 1 : 2; +#ifdef _BT_USE_CENTER_LIMIT_ + limit_err = m_limit.getCorrection() * m_referenceSign; +#else + limit_err = m_correction * m_referenceSign; +#endif + limit = (limit_err > btScalar(0.0)) ? 1 : 2; + } // if the hinge has joint limits or motor, add in the extra row int powered = 0; @@ -514,7 +538,11 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf info->m_upperLimit[srow] = 0; } // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) +#ifdef _BT_USE_CENTER_LIMIT_ + btScalar bounce = m_limit.getRelaxationFactor(); +#else btScalar bounce = m_relaxationFactor; +#endif if(bounce > btScalar(0.0)) { btScalar vel = angVelA.dot(ax1); @@ -544,7 +572,11 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf } } } +#ifdef _BT_USE_CENTER_LIMIT_ + info->m_constraintError[srow] *= m_limit.getBiasFactor(); +#else info->m_constraintError[srow] *= m_biasFactor; +#endif } // if(limit) } // if angular limit or powered } @@ -581,38 +613,14 @@ btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTran } -#if 0 -void btHingeConstraint::testLimit() -{ - // Compute limit information - m_hingeAngle = getHingeAngle(); - m_correction = btScalar(0.); - m_limitSign = btScalar(0.); - m_solveLimit = false; - if (m_lowerLimit <= m_upperLimit) - { - if (m_hingeAngle <= m_lowerLimit) - { - m_correction = (m_lowerLimit - m_hingeAngle); - m_limitSign = 1.0f; - m_solveLimit = true; - } - else if (m_hingeAngle >= m_upperLimit) - { - m_correction = m_upperLimit - m_hingeAngle; - m_limitSign = -1.0f; - m_solveLimit = true; - } - } - return; -} -#else - void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB) { // Compute limit information m_hingeAngle = getHingeAngle(transA,transB); +#ifdef _BT_USE_CENTER_LIMIT_ + m_limit.test(m_hingeAngle); +#else m_correction = btScalar(0.); m_limitSign = btScalar(0.); m_solveLimit = false; @@ -632,9 +640,10 @@ void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& t m_solveLimit = true; } } +#endif return; } -#endif + static btVector3 vHinge(0, 0, btScalar(1)); @@ -665,6 +674,9 @@ void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt) void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt) { +#ifdef _BT_USE_CENTER_LIMIT_ + m_limit.fit(targetAngle); +#else if (m_lowerLimit < m_upperLimit) { if (targetAngle < m_lowerLimit) @@ -672,7 +684,7 @@ void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt) else if (targetAngle > m_upperLimit) targetAngle = m_upperLimit; } - +#endif // compute angular velocity btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); btScalar dAngle = targetAngle - curAngle; @@ -843,8 +855,13 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info int limit = 0; if(getSolveLimit()) { - limit_err = m_correction * m_referenceSign; - limit = (limit_err > btScalar(0.0)) ? 1 : 2; +#ifdef _BT_USE_CENTER_LIMIT_ + limit_err = m_limit.getCorrection() * m_referenceSign; +#else + limit_err = m_correction * m_referenceSign; +#endif + limit = (limit_err > btScalar(0.0)) ? 1 : 2; + } // if the hinge has joint limits or motor, add in the extra row int powered = 0; @@ -908,7 +925,11 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info info->m_upperLimit[srow] = 0; } // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) +#ifdef _BT_USE_CENTER_LIMIT_ + btScalar bounce = m_limit.getRelaxationFactor(); +#else btScalar bounce = m_relaxationFactor; +#endif if(bounce > btScalar(0.0)) { btScalar vel = angVelA.dot(ax1); @@ -938,7 +959,11 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info } } } +#ifdef _BT_USE_CENTER_LIMIT_ + info->m_constraintError[srow] *= m_limit.getBiasFactor(); +#else info->m_constraintError[srow] *= m_biasFactor; +#endif } // if(limit) } // if angular limit or powered } diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index c38b64045..8431926e4 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -17,6 +17,8 @@ subject to the following restrictions: #ifndef HINGECONSTRAINT_H #define HINGECONSTRAINT_H +#define _BT_USE_CENTER_LIMIT_ 1 + #include "LinearMath/btVector3.h" #include "btJacobianEntry.h" @@ -33,6 +35,7 @@ class btRigidBody; #endif //BT_USE_DOUBLE_PRECISION + enum btHingeFlags { BT_HINGE_FLAGS_CFM_STOP = 1, @@ -57,25 +60,31 @@ public: btScalar m_motorTargetVelocity; btScalar m_maxMotorImpulse; - btScalar m_limitSoftness; - btScalar m_biasFactor; - btScalar m_relaxationFactor; - - btScalar m_lowerLimit; - btScalar m_upperLimit; - - btScalar m_kHinge; +#ifdef _BT_USE_CENTER_LIMIT_ + btAngularLimit m_limit; +#else + btScalar m_lowerLimit; + btScalar m_upperLimit; btScalar m_limitSign; btScalar m_correction; + btScalar m_limitSoftness; + btScalar m_biasFactor; + btScalar m_relaxationFactor; + + bool m_solveLimit; +#endif + + btScalar m_kHinge; + + btScalar m_accLimitImpulse; btScalar m_hingeAngle; - btScalar m_referenceSign; + btScalar m_referenceSign; bool m_angularOnly; bool m_enableAngularMotor; - bool m_solveLimit; bool m_useSolveConstraintObsolete; bool m_useOffsetForConstraintFrame; bool m_useReferenceFrameA; @@ -169,13 +178,15 @@ public: void setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f) { +#ifdef _BT_USE_CENTER_LIMIT_ + m_limit.set(low, high, _softness, _biasFactor, _relaxationFactor); +#else m_lowerLimit = btNormalizeAngle(low); m_upperLimit = btNormalizeAngle(high); - m_limitSoftness = _softness; m_biasFactor = _biasFactor; m_relaxationFactor = _relaxationFactor; - +#endif } void setAxis(btVector3& axisInA) @@ -202,12 +213,20 @@ public: btScalar getLowerLimit() const { - return m_lowerLimit; +#ifdef _BT_USE_CENTER_LIMIT_ + return m_limit.getLow(); +#else + return m_lowerLimit; +#endif } btScalar getUpperLimit() const { - return m_upperLimit; +#ifdef _BT_USE_CENTER_LIMIT_ + return m_limit.getHigh(); +#else + return m_upperLimit; +#endif } @@ -226,12 +245,20 @@ public: inline int getSolveLimit() { - return m_solveLimit; +#ifdef _BT_USE_CENTER_LIMIT_ + return m_limit.isLimit(); +#else + return m_solveLimit; +#endif } inline btScalar getLimitSign() { +#ifdef _BT_USE_CENTER_LIMIT_ + return m_limit.getSign(); +#else return m_limitSign; +#endif } inline bool getAngularOnly() @@ -330,12 +357,19 @@ SIMD_FORCE_INLINE const char* btHingeConstraint::serialize(void* dataBuffer, btS hingeData->m_maxMotorImpulse = float(m_maxMotorImpulse); hingeData->m_motorTargetVelocity = float(m_motorTargetVelocity); hingeData->m_useReferenceFrameA = m_useReferenceFrameA; - +#ifdef _BT_USE_CENTER_LIMIT_ + hingeData->m_lowerLimit = float(m_limit.getLow()); + hingeData->m_upperLimit = float(m_limit.getHigh()); + hingeData->m_limitSoftness = float(m_limit.getSoftness()); + hingeData->m_biasFactor = float(m_limit.getBiasFactor()); + hingeData->m_relaxationFactor = float(m_limit.getRelaxationFactor()); +#else hingeData->m_lowerLimit = float(m_lowerLimit); hingeData->m_upperLimit = float(m_upperLimit); hingeData->m_limitSoftness = float(m_limitSoftness); hingeData->m_biasFactor = float(m_biasFactor); hingeData->m_relaxationFactor = float(m_relaxationFactor); +#endif return btHingeConstraintDataName; } diff --git a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp index ce43b5369..8212c36ff 100644 --- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp @@ -140,3 +140,71 @@ btRigidBody& btTypedConstraint::getFixedBody() return s_fixed; } + +void btAngularLimit::set(btScalar low, btScalar high, btScalar _softness, btScalar _biasFactor, btScalar _relaxationFactor) +{ + m_halfRange = (high - low) / 2.0f; + m_center = btNormalizeAngle(low + m_halfRange); + m_softness = _softness; + m_biasFactor = _biasFactor; + m_relaxationFactor = _relaxationFactor; +} + +void btAngularLimit::test(const btScalar angle) +{ + m_correction = 0.0f; + m_sign = 0.0f; + m_solveLimit = false; + + if (m_halfRange >= 0.0f) + { + btScalar deviation = btNormalizeAngle(angle - m_center); + if (deviation < -m_halfRange) + { + m_solveLimit = true; + m_correction = - (deviation + m_halfRange); + m_sign = +1.0f; + } + else if (deviation > m_halfRange) + { + m_solveLimit = true; + m_correction = m_halfRange - deviation; + m_sign = -1.0f; + } + } +} + + +btScalar btAngularLimit::getError() const +{ + return m_correction * m_sign; +} + +void btAngularLimit::fit(btScalar& angle) const +{ + if (m_halfRange > 0.0f) + { + btScalar relativeAngle = btNormalizeAngle(angle - m_center); + if (!btEqual(relativeAngle, m_halfRange)) + { + if (relativeAngle > 0.0f) + { + angle = getHigh(); + } + else + { + angle = getLow(); + } + } + } +} + +btScalar btAngularLimit::getLow() const +{ + return btNormalizeAngle(m_center - m_halfRange); +} + +btScalar btAngularLimit::getHigh() const +{ + return btNormalizeAngle(m_center + m_halfRange); +} diff --git a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h index 1116c2686..f1eb955bf 100644 --- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -313,5 +313,98 @@ SIMD_FORCE_INLINE int btTypedConstraint::calculateSerializeBufferSize() const +class btAngularLimit +{ +private: + btScalar + m_center, + m_halfRange, + m_softness, + m_biasFactor, + m_relaxationFactor, + m_correction, + m_sign; + + bool + m_solveLimit; + +public: + /// Default constructor initializes limit as inactive, allowing free constraint movement + btAngularLimit() + :m_center(0.0f), + m_halfRange(-1.0f), + m_softness(0.9f), + m_biasFactor(0.3f), + m_relaxationFactor(1.0f), + m_correction(0.0f), + m_sign(0.0f), + m_solveLimit(false) + {} + + /// Sets all limit's parameters. + /// When low > high limit becomes inactive. + /// When high - low > 2PI limit is ineffective too becouse no angle can exceed the limit + void set(btScalar low, btScalar high, btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f); + + /// Checks conastaint angle against limit. If limit is active and the angle violates the limit + /// correction is calculated. + void test(const btScalar angle); + + /// Returns limit's softness + inline btScalar getSoftness() const + { + return m_softness; + } + + /// Returns limit's bias factor + inline btScalar getBiasFactor() const + { + return m_biasFactor; + } + + /// Returns limit's relaxation factor + inline btScalar getRelaxationFactor() const + { + return m_relaxationFactor; + } + + /// Returns correction value evaluated when test() was invoked + inline btScalar getCorrection() const + { + return m_correction; + } + + /// Returns sign value evaluated when test() was invoked + inline btScalar getSign() const + { + return m_sign; + } + + /// Gives half of the distance between min and max limit angle + inline btScalar getHalfRange() const + { + return m_halfRange; + } + + /// Returns true when the last test() invocation recognized limit violation + inline bool isLimit() const + { + return m_solveLimit; + } + + /// Checks given angle against limit. If limit is active and angle doesn't fit it, the angle + /// returned is modified so it equals to the limit closest to given angle. + void fit(btScalar& angle) const; + + /// Returns correction value multiplied by sign value + btScalar getError() const; + + inline btScalar getLow() const; + + inline btScalar getHigh() const; + +}; + + #endif //TYPED_CONSTRAINT_H