From ee664e7750616bc8baf140f7bb770e42f8ec771b Mon Sep 17 00:00:00 2001 From: rponom Date: Tue, 2 Jun 2009 01:10:43 +0000 Subject: [PATCH] Damping for spring motors added see btGeneric6DofSpringConstraint::setDamping() --- Demos/ConstraintDemo/ConstraintDemo.cpp | 2 ++ .../btGeneric6DofSpringConstraint.cpp | 16 +++++++++++++--- .../btGeneric6DofSpringConstraint.h | 2 ++ .../ConstraintSolver/btHinge2Constraint.cpp | 1 + .../btSequentialImpulseConstraintSolver.cpp | 1 + .../ConstraintSolver/btTypedConstraint.h | 2 ++ 6 files changed, 21 insertions(+), 3 deletions(-) diff --git a/Demos/ConstraintDemo/ConstraintDemo.cpp b/Demos/ConstraintDemo/ConstraintDemo.cpp index 7b298eaa2..e474edfd4 100644 --- a/Demos/ConstraintDemo/ConstraintDemo.cpp +++ b/Demos/ConstraintDemo/ConstraintDemo.cpp @@ -383,8 +383,10 @@ void ConstraintDemo::initPhysics() pGen6DOFSpring->enableSpring(0, true); pGen6DOFSpring->setStiffness(0, 39.478f); + pGen6DOFSpring->setDamping(0, 0.5f); pGen6DOFSpring->enableSpring(5, true); pGen6DOFSpring->setStiffness(5, 39.478f); + pGen6DOFSpring->setDamping(0, 0.3f); pGen6DOFSpring->setEquilibriumPoint(); } #endif diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp index 49c9309f9..d75774316 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp @@ -26,6 +26,7 @@ btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, b m_springEnabled[i] = false; m_equilibriumPoint[i] = btScalar(0.f); m_springStiffness[i] = btScalar(0.f); + m_springDamping[i] = btScalar(1.f); } } @@ -53,6 +54,13 @@ void btGeneric6DofSpringConstraint::setStiffness(int index, btScalar stiffness) } +void btGeneric6DofSpringConstraint::setDamping(int index, btScalar damping) +{ + btAssert((index >= 0) && (index < 6)); + m_springDamping[index] = damping; +} + + void btGeneric6DofSpringConstraint::setEquilibriumPoint() { calculateTransforms(); @@ -99,8 +107,9 @@ void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* inf btScalar delta = currPos - m_equilibriumPoint[i]; // spring force is (delta * m_stiffness) according to Hooke's Law btScalar force = delta * m_springStiffness[i]; - m_linearLimits.m_targetVelocity[i] = force * info->fps; - m_linearLimits.m_maxMotorForce[i] = btFabs(force) / info->fps; + btScalar velFactor = info->fps * m_springDamping[i] / btScalar(info->m_numIterations); + m_linearLimits.m_targetVelocity[i] = velFactor * force; + m_linearLimits.m_maxMotorForce[i] = btFabs(force) / info->fps; } } for(i = 0; i < 3; i++) @@ -113,7 +122,8 @@ void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* inf btScalar delta = currPos - m_equilibriumPoint[i+3]; // spring force is (-delta * m_stiffness) according to Hooke's Law btScalar force = -delta * m_springStiffness[i+3]; - m_angularLimits[i].m_targetVelocity = force * info->fps; + btScalar velFactor = info->fps * m_springDamping[i+3] / btScalar(info->m_numIterations); + m_angularLimits[i].m_targetVelocity = velFactor * force; m_angularLimits[i].m_maxMotorForce = btFabs(force) / info->fps; } } diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h index afbbcddfb..b084c7e8d 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h @@ -38,11 +38,13 @@ protected: bool m_springEnabled[6]; btScalar m_equilibriumPoint[6]; btScalar m_springStiffness[6]; + btScalar m_springDamping[6]; // between 0 and 1 (1 == no damping) void internalUpdateSprings(btConstraintInfo2* info); public: btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); void enableSpring(int index, bool onOff); void setStiffness(int index, btScalar stiffness); + void setDamping(int index, btScalar damping); void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF void setEquilibriumPoint(int index); // set the current constraint position/orientation as an equilibrium point for given DOF virtual void getInfo2 (btConstraintInfo2* info); diff --git a/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp b/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp index 887a85171..4d9244134 100644 --- a/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp @@ -60,6 +60,7 @@ btHinge2Constraint::btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVec // enable suspension enableSpring(2, true); setStiffness(2, SIMD_PI * SIMD_PI * 4.f); // period 1 sec for 1 kilogramm weel :-) + setDamping(2, 0.01f); setEquilibriumPoint(); } diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 97674a4e5..58bc02f8e 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -730,6 +730,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol info2.cfm = ¤tConstraintRow->m_cfm; info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit; info2.m_upperLimit = ¤tConstraintRow->m_upperLimit; + info2.m_numIterations = infoGlobal.m_numIterations; constraints[i]->getInfo2(&info2); ///finalize the constraint setup diff --git a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h index c2e5ad862..4b62b8437 100644 --- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -97,6 +97,8 @@ public: // note that the returned indexes are relative to the first index of // the constraint. int *findex; + // number of solver iterations + int m_numIterations; }; ///internal method used by the constraint solver, don't use them directly