diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index c93789f11..29c8496c3 100644 --- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -184,6 +184,7 @@ void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info) J2[srow+1] = -ax1[1]; J2[srow+2] = -ax1[2]; btScalar k = info->fps * m_biasFactor; + info->m_constraintError[srow] = k * m_swingCorrection; info->cfm[srow] = 0.0f; // m_swingCorrection is always positive or 0 @@ -659,7 +660,7 @@ void btConeTwistConstraint::calcAngleInfo2() btScalar z = ivB.dot(kvA); if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) { // fixed. We'll need to add one more row to constraint - if((y != btScalar(0.f)) || (z != btScalar(0.f))) + if((!btFuzzyZero(y)) || (!(btFuzzyZero(z)))) { m_solveSwingLimit = true; m_swingAxis = -ivB.cross(ivA); @@ -669,7 +670,7 @@ void btConeTwistConstraint::calcAngleInfo2() { if(m_swingSpan1 < m_fixThresh) { // hinge around Y axis - if(y != btScalar(0.f)) + if(!(btFuzzyZero(y))) { m_solveSwingLimit = true; if(m_swingSpan2 >= m_fixThresh) @@ -691,7 +692,7 @@ void btConeTwistConstraint::calcAngleInfo2() } else { // hinge around Z axis - if(z != btScalar(0.f)) + if(!btFuzzyZero(z)) { m_solveSwingLimit = true; if(m_swingSpan1 >= m_fixThresh) diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h index ae4353153..84ea9e040 100644 --- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h @@ -122,7 +122,7 @@ public: { case 3: { - m_swingSpan1 = limitValue; + m_twistSpan = limitValue; break; } case 4: @@ -132,7 +132,7 @@ public: } case 5: { - m_twistSpan = limitValue; + m_swingSpan1 = limitValue; break; } default: @@ -141,7 +141,7 @@ public: }; } - void setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 0.8f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f) + void setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f) { m_swingSpan1 = _swingSpan1; m_swingSpan2 = _swingSpan2;