From 0024c87316b9a85b2f0b048c56eba8e0db40292f Mon Sep 17 00:00:00 2001 From: "erwin.coumans" Date: Tue, 22 Oct 2013 21:49:52 +0000 Subject: [PATCH] move the m_maxAppliedImpulse into base class, and use it for motor strength/point to point constraint strength --- .../FeatherstoneMultiBodyDemo.cpp | 2 +- .../Featherstone/btMultiBodyConstraint.cpp | 7 ++++--- .../Featherstone/btMultiBodyConstraint.h | 11 +++++++++++ .../Featherstone/btMultiBodyJointLimitConstraint.cpp | 2 +- .../Featherstone/btMultiBodyJointMotor.cpp | 6 +++--- .../Featherstone/btMultiBodyJointMotor.h | 2 +- .../Featherstone/btMultiBodyPoint2Point.cpp | 6 ++---- .../Featherstone/btMultiBodyPoint2Point.h | 11 ++--------- 8 files changed, 25 insertions(+), 22 deletions(-) diff --git a/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp b/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp index 3affd399a..bf6b55016 100644 --- a/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp +++ b/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp @@ -194,7 +194,7 @@ void FeatherstoneMultiBodyDemo::initPhysics() } - btMultiBody* mbA = createFeatherstoneMultiBody(world, 4, btVector3 (60,29.5,-2)*scaling, false, true,true,true); + btMultiBody* mbA = createFeatherstoneMultiBody(world, 2, btVector3 (60,29.5,-2)*scaling, false, true,true,true); int numLinks = 10; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index 3fbc3b0a0..8053ee770 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -7,7 +7,8 @@ btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bod m_linkA(linkA), m_linkB(linkB), m_num_rows(numRows), - m_isUnilateral(isUnilateral) + m_isUnilateral(isUnilateral), + m_maxAppliedImpulse(100) { m_jac_size_A = (6 + bodyA->getNumLinks()); m_jac_size_both = (m_jac_size_A + (bodyB ? 6 + bodyB->getNumLinks() : 0)); @@ -519,8 +520,8 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr } solverConstraint.m_cfm = 0.f; - solverConstraint.m_lowerLimit = 0; - solverConstraint.m_upperLimit = 1e10f; + solverConstraint.m_lowerLimit = -m_maxAppliedImpulse; + solverConstraint.m_upperLimit = m_maxAppliedImpulse; } } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 8f744c4e4..97f5486d6 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -55,6 +55,9 @@ protected: bool m_isUnilateral; + btScalar m_maxAppliedImpulse; + + // data block laid out as follows: // cached impulses. (one per row.) // jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc) @@ -147,6 +150,14 @@ public: return &m_data[m_num_rows + (row * m_jac_size_both) + m_jac_size_A]; } + btScalar getMaxAppliedImpulse() const + { + return m_maxAppliedImpulse; + } + void setMaxAppliedImpulse(btScalar maxImp) + { + m_maxAppliedImpulse = maxImp; + } }; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp index 5ef65dc71..5eb40f14b 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp @@ -90,7 +90,7 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint constraintRow.m_multiBodyA = m_bodyA; constraintRow.m_multiBodyB = m_bodyB; - btScalar rel_vel = fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,0,0,1e30); + btScalar rel_vel = fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,0,-m_maxAppliedImpulse,m_maxAppliedImpulse); { btScalar penetration = getPosition(row); btScalar positionalError = 0.f; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp index 2aeb8018c..c20663ff4 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -23,9 +23,9 @@ subject to the following restrictions: btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse) :btMultiBodyConstraint(body,body,link,link,1,true), - m_desiredVelocity(desiredVelocity), - m_maxMotorImpulse(maxMotorImpulse) + m_desiredVelocity(desiredVelocity) { + m_maxAppliedImpulse = maxMotorImpulse; // the data.m_jacobians never change, so may as well // initialize them here @@ -82,7 +82,7 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); btScalar penetration = 0; - fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,m_desiredVelocity,-m_maxMotorImpulse,m_maxMotorImpulse); + fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,m_desiredVelocity,-m_maxAppliedImpulse,m_maxAppliedImpulse); } } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h index 06b476b6f..61ee4663b 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h @@ -25,7 +25,7 @@ class btMultiBodyJointMotor : public btMultiBodyConstraint { protected: - btScalar m_maxMotorImpulse; + btScalar m_desiredVelocity; public: diff --git a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp index 7b89bf2ce..a16455bfb 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp @@ -24,8 +24,7 @@ btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRi m_rigidBodyA(0), m_rigidBodyB(bodyB), m_pivotInA(pivotInA), - m_pivotInB(pivotInB), - m_maxAppliedImpulse(1e30f) + m_pivotInB(pivotInB) { } @@ -34,8 +33,7 @@ btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, bt m_rigidBodyA(0), m_rigidBodyB(0), m_pivotInA(pivotInA), - m_pivotInB(pivotInB), - m_maxAppliedImpulse(1e30f) + m_pivotInB(pivotInB) { } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h index 3cefa957a..824a57393 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h @@ -28,7 +28,7 @@ protected: btRigidBody* m_rigidBodyB; btVector3 m_pivotInA; btVector3 m_pivotInB; - btScalar m_maxAppliedImpulse; + public: @@ -54,14 +54,7 @@ public: m_pivotInB = pivotInB; } - btScalar getMaxAppliedImpulse() const - { - return m_maxAppliedImpulse; - } - void setMaxAppliedImpulse(btScalar maxImp) - { - m_maxAppliedImpulse = maxImp; - } + }; #endif //BT_MULTIBODY_POINT2POINT_H