diff --git a/Demos/ConstraintDemo/ConstraintDemo.cpp b/Demos/ConstraintDemo/ConstraintDemo.cpp index e474edfd4..4af3ab822 100644 --- a/Demos/ConstraintDemo/ConstraintDemo.cpp +++ b/Demos/ConstraintDemo/ConstraintDemo.cpp @@ -169,10 +169,12 @@ void ConstraintDemo::initPhysics() slider->setLinearUpperLimit(hiSliderLimit); //range should be small, otherwise singularities will 'explode' the constraint - slider->setAngularLowerLimit(btVector3(-1.5,0,0)); - slider->setAngularUpperLimit(btVector3(1.5,0,0)); +// slider->setAngularLowerLimit(btVector3(-1.5,0,0)); +// slider->setAngularUpperLimit(btVector3(1.5,0,0)); // slider->setAngularLowerLimit(btVector3(0,0,0)); // slider->setAngularUpperLimit(btVector3(0,0,0)); + slider->setAngularLowerLimit(btVector3(-SIMD_PI,0,0)); + slider->setAngularUpperLimit(btVector3(1.5,0,0)); slider->getTranslationalLimitMotor()->m_enableMotor[0] = true; slider->getTranslationalLimitMotor()->m_targetVelocity[0] = -5.0f; @@ -198,7 +200,11 @@ void ConstraintDemo::initPhysics() spDoorHinge = new btHingeConstraint( *pDoorBody, btPivotA, btAxisA ); - spDoorHinge->setLimit( 0.0f, M_PI_2 ); +// spDoorHinge->setLimit( 0.0f, M_PI_2 ); + // test problem values +// spDoorHinge->setLimit( -M_PI, M_PI*0.8f); + spDoorHinge->setLimit( -M_PI*0.8f, M_PI); +// spDoorHinge->setLimit( 0.0f, 0.0f ); m_dynamicsWorld->addConstraint(spDoorHinge); spDoorHinge->setDbgDrawSize(btScalar(5.f)); diff --git a/Demos/SliderConstraintDemo/SliderConstraintDemo.cpp b/Demos/SliderConstraintDemo/SliderConstraintDemo.cpp index 2054ab665..345d42706 100755 --- a/Demos/SliderConstraintDemo/SliderConstraintDemo.cpp +++ b/Demos/SliderConstraintDemo/SliderConstraintDemo.cpp @@ -213,11 +213,13 @@ void SliderConstraintDemo::initPhysics() // spSlider1->setUpperLinLimit(15.0F); // spSlider1->setLowerLinLimit(-10.0F); // spSlider1->setUpperLinLimit(-10.0F); + spSlider1->setLowerAngLimit(-SIMD_PI / 3.0F); spSlider1->setUpperAngLimit( SIMD_PI / 3.0F); #endif m_dynamicsWorld->addConstraint(spSlider1, true); + spSlider1->setDbgDrawSize(btScalar(5.f)); // add kinematic rigid body A2 worldPos.setValue(0,2,0); @@ -251,8 +253,13 @@ void SliderConstraintDemo::initPhysics() // spSlider2->setLowerAngLimit(SIMD_PI / 2.0F); // spSlider2->setUpperAngLimit(-SIMD_PI / 2.0F); - spSlider2->setLowerAngLimit(-SIMD_PI / 2.0F); - spSlider2->setUpperAngLimit(SIMD_PI / 2.0F); + + // spSlider2->setLowerAngLimit(-SIMD_PI / 2.0F); +// spSlider2->setUpperAngLimit(SIMD_PI / 2.0F); + spSlider2->setLowerAngLimit(-SIMD_PI); + spSlider2->setUpperAngLimit(SIMD_PI *0.8F); + + // spSlider2->setLowerAngLimit(-0.01F); // spSlider2->setUpperAngLimit(0.01F); @@ -285,6 +292,7 @@ void SliderConstraintDemo::initPhysics() // spSlider2->setSoftnessLimAng(0.1); #endif m_dynamicsWorld->addConstraint(spSlider2, true); + spSlider2->setDbgDrawSize(btScalar(5.f)); #if 1 @@ -306,6 +314,8 @@ void SliderConstraintDemo::initPhysics() btVector3 pivB(-2.5, 0., 0.); spP2PConst = new btPoint2PointConstraint(*pRbA3, *pRbB3, pivA, pivB); m_dynamicsWorld->addConstraint(spP2PConst, true); + spP2PConst->setDbgDrawSize(btScalar(5.f)); + } #endif @@ -335,6 +345,8 @@ void SliderConstraintDemo::initPhysics() spHingeConst->enableAngularMotor(true, 10.0, 0.19); m_dynamicsWorld->addConstraint(spHingeConst, true); + spHingeConst->setDbgDrawSize(btScalar(5.f)); + #endif } // SliderConstraintDemo::initPhysics() diff --git a/Demos/SliderConstraintDemo/main.cpp b/Demos/SliderConstraintDemo/main.cpp index f84bee370..54f1afabe 100755 --- a/Demos/SliderConstraintDemo/main.cpp +++ b/Demos/SliderConstraintDemo/main.cpp @@ -17,6 +17,7 @@ int main(int argc,char** argv) sliderConstraintDemo->initPhysics(); sliderConstraintDemo->getDynamicsWorld()->setDebugDrawer(&gDebugDrawer); + sliderConstraintDemo->setDebugMode(btIDebugDraw::DBG_DrawConstraints+btIDebugDraw::DBG_DrawConstraintLimits); return glutmain(argc, argv,640,480,"Slider Constraint Demo. http://www.continuousphysics.com/Bullet/phpBB2/", sliderConstraintDemo); diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index 58065b00d..381c3c177 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -112,7 +112,6 @@ int btRotationalLimitMotor::testLimitValue(btScalar test_value) m_currentLimit = 0;//Free from violation return 0; } - if (test_value < m_loLimit) { m_currentLimit = 1;//low limit violation @@ -420,6 +419,7 @@ void btGeneric6DofConstraint::buildAngularJacobian( bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index) { btScalar angle = m_calculatedAxisAngleDiff[axis_index]; + angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit); m_angularLimits[axis_index].m_currentPosition = angle; //test limits m_angularLimits[axis_index].testLimitValue(angle); diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h index 29b85eed1..cf7f21a7f 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -65,8 +65,8 @@ public: m_targetVelocity = 0; m_maxMotorForce = 0.1f; m_maxLimitForce = 300.0f; - m_loLimit = -SIMD_INFINITY; - m_hiLimit = SIMD_INFINITY; + m_loLimit = 1.0f; + m_hiLimit = -1.0f; m_ERP = 0.5f; m_bounce = 0.0f; m_damping = 1.0f; @@ -412,16 +412,14 @@ public: void setAngularLowerLimit(const btVector3& angularLower) { - m_angularLimits[0].m_loLimit = angularLower.getX(); - m_angularLimits[1].m_loLimit = angularLower.getY(); - m_angularLimits[2].m_loLimit = angularLower.getZ(); + for(int i = 0; i < 3; i++) + m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]); } void setAngularUpperLimit(const btVector3& angularUpper) { - m_angularLimits[0].m_hiLimit = angularUpper.getX(); - m_angularLimits[1].m_hiLimit = angularUpper.getY(); - m_angularLimits[2].m_hiLimit = angularUpper.getZ(); + for(int i = 0; i < 3; i++) + m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]); } //! Retrieves the angular limit informacion @@ -446,6 +444,8 @@ public: } else { + lo = btNormalizeAngle(lo); + hi = btNormalizeAngle(hi); m_angularLimits[axis-3].m_loLimit = lo; m_angularLimits[axis-3].m_hiLimit = hi; } diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index f8129cc33..d769c918e 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -79,8 +79,8 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); //start with free - m_lowerLimit = btScalar(BT_LARGE_FLOAT); - m_upperLimit = btScalar(-BT_LARGE_FLOAT); + m_lowerLimit = btScalar(1.0f); + m_upperLimit = btScalar(-1.0f); m_biasFactor = 0.3f; m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; @@ -119,8 +119,8 @@ m_useReferenceFrameA(useReferenceFrameA) rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); //start with free - m_lowerLimit = btScalar(BT_LARGE_FLOAT); - m_upperLimit = btScalar(-BT_LARGE_FLOAT); + m_lowerLimit = btScalar(1.0f); + m_upperLimit = btScalar(-1.0f); m_biasFactor = 0.3f; m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; @@ -139,8 +139,8 @@ m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useReferenceFrameA(useReferenceFrameA) { //start with free - m_lowerLimit = btScalar(BT_LARGE_FLOAT); - m_upperLimit = btScalar(-BT_LARGE_FLOAT); + m_lowerLimit = btScalar(1.0f); + m_upperLimit = btScalar(-1.0f); m_biasFactor = 0.3f; m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; @@ -162,8 +162,8 @@ m_useReferenceFrameA(useReferenceFrameA) m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin()); //start with free - m_lowerLimit = btScalar(BT_LARGE_FLOAT); - m_upperLimit = btScalar(-BT_LARGE_FLOAT); + m_lowerLimit = btScalar(1.0f); + m_upperLimit = btScalar(-1.0f); m_biasFactor = 0.3f; m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; @@ -648,7 +648,7 @@ btScalar btHingeConstraint::getHingeAngle() } - +#if 0 void btHingeConstraint::testLimit() { // Compute limit information @@ -673,8 +673,36 @@ void btHingeConstraint::testLimit() } return; } +#else +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) + { + m_hingeAngle = btAdjustAngleToLimits(m_hingeAngle, 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; +} +#endif + static btVector3 vHinge(0, 0, btScalar(1)); void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt) diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index 8d464e1fc..6c5badeb0 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -129,8 +129,8 @@ public: void setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f) { - m_lowerLimit = low; - m_upperLimit = high; + m_lowerLimit = btNormalizeAngle(low); + m_upperLimit = btNormalizeAngle(high); m_limitSoftness = _softness; m_biasFactor = _biasFactor; diff --git a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp index 133aed727..6e59a4c6a 100755 --- a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp @@ -799,6 +799,7 @@ void btSliderConstraint::testAngLimits(void) const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2); const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1); btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); + rot = btAdjustAngleToLimits(rot, m_lowerAngLimit, m_upperAngLimit); m_angPos = rot; if(rot < m_lowerAngLimit) { diff --git a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h index 01cef59ed..499c3bd3f 100755 --- a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h @@ -151,9 +151,9 @@ public: btScalar getUpperLinLimit() { return m_upperLinLimit; } void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; } btScalar getLowerAngLimit() { return m_lowerAngLimit; } - void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = lowerLimit; } + void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = btNormalizeAngle(lowerLimit); } btScalar getUpperAngLimit() { return m_upperAngLimit; } - void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = upperLimit; } + void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = btNormalizeAngle(upperLimit); } bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; } btScalar getSoftnessDirLin() { return m_softnessDirLin; } btScalar getRestitutionDirLin() { return m_restitutionDirLin; } diff --git a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h index 4b62b8437..4fbca04aa 100644 --- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -244,4 +244,31 @@ public: }; +// returns angle in range [-SIMD_2_PI, SIMD_2_PI], closest to one of the limits +// all arguments should be normalized angles (i.e. in range [-SIMD_PI, SIMD_PI]) +SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians) +{ + if(angleLowerLimitInRadians >= angleUpperLimitInRadians) + { + return angleInRadians; + } + else if(angleInRadians < angleLowerLimitInRadians) + { + btScalar diffLo = btNormalizeAngle(angleLowerLimitInRadians - angleInRadians); // this is positive + btScalar diffHi = btFabs(btNormalizeAngle(angleUpperLimitInRadians - angleInRadians)); + return (diffLo < diffHi) ? angleInRadians : (angleInRadians + SIMD_2_PI); + } + else if(angleInRadians > angleUpperLimitInRadians) + { + btScalar diffHi = btNormalizeAngle(angleInRadians - angleUpperLimitInRadians); // this is positive + btScalar diffLo = btFabs(btNormalizeAngle(angleInRadians - angleLowerLimitInRadians)); + return (diffLo < diffHi) ? (angleInRadians - SIMD_2_PI) : angleInRadians; + } + else + { + return angleInRadians; + } +} + + #endif //TYPED_CONSTRAINT_H diff --git a/src/LinearMath/btScalar.h b/src/LinearMath/btScalar.h index 6261922cd..544b28feb 100644 --- a/src/LinearMath/btScalar.h +++ b/src/LinearMath/btScalar.h @@ -195,6 +195,7 @@ SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2(x, y); SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return exp(x); } SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return log(x); } SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return pow(x,y); } +SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmod(x,y); } #else @@ -232,6 +233,7 @@ SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y) SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return expf(x); } SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return logf(x); } SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); } +SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); } #endif @@ -430,5 +432,22 @@ SIMD_FORCE_INLINE double btUnswapEndianDouble(const unsigned char *src) return d; } +// returns normalized value in range [-SIMD_PI, SIMD_PI] +SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians) +{ + angleInRadians = btFmod(angleInRadians, SIMD_2_PI); + if(angleInRadians < -SIMD_PI) + { + return angleInRadians + SIMD_2_PI; + } + else if(angleInRadians > SIMD_PI) + { + return angleInRadians - SIMD_2_PI; + } + else + { + return angleInRadians; + } +} #endif //SIMD___SCALAR_H