diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp')
| -rw-r--r-- | extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp | 130 |
1 files changed, 64 insertions, 66 deletions
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp index 3f05aa4d5fa..94b36ac108c 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp @@ -20,21 +20,18 @@ subject to the following restrictions: #include "btMultiBodyLinkCollider.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" - - btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper) //:btMultiBodyConstraint(body,0,link,-1,2,true), - :btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,2,true), - m_lowerBound(lower), - m_upperBound(upper) + : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 2, true, MULTIBODY_CONSTRAINT_LIMIT), + m_lowerBound(lower), + m_upperBound(upper) { - } void btMultiBodyJointLimitConstraint::finalizeMultiDof() { // the data.m_jacobians never change, so may as well - // initialize them here + // initialize them here allocateJacobiansMultiDof(); @@ -55,15 +52,18 @@ btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint() int btMultiBodyJointLimitConstraint::getIslandIdA() const { - if(m_bodyA) + if (m_bodyA) { - btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); - if (col) - return col->getIslandTag(); - for (int i=0;i<m_bodyA->getNumLinks();i++) + if (m_linkA < 0) + { + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + } + else { - if (m_bodyA->getLink(i).m_collider) - return m_bodyA->getLink(i).m_collider->getIslandTag(); + if (m_bodyA->getLink(m_linkA).m_collider) + return m_bodyA->getLink(m_linkA).m_collider->getIslandTag(); } } return -1; @@ -71,82 +71,86 @@ int btMultiBodyJointLimitConstraint::getIslandIdA() const int btMultiBodyJointLimitConstraint::getIslandIdB() const { - if(m_bodyB) + if (m_bodyB) { - btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); - if (col) - return col->getIslandTag(); - - for (int i=0;i<m_bodyB->getNumLinks();i++) + if (m_linkB < 0) { - col = m_bodyB->getLink(i).m_collider; + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); if (col) return col->getIslandTag(); } + else + { + if (m_bodyB->getLink(m_linkB).m_collider) + return m_bodyB->getLink(m_linkB).m_collider->getIslandTag(); + } } return -1; } - void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal) + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal) { - - // only positions need to be updated -- data.m_jacobians and force - // directions were set in the ctor and never change. + // only positions need to be updated -- data.m_jacobians and force + // directions were set in the ctor and never change. if (m_numDofsFinalized != m_jacSizeBoth) { - finalizeMultiDof(); + finalizeMultiDof(); } + // row 0: the lower bound + setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); //multidof: this is joint-type dependent - // row 0: the lower bound - setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); //multidof: this is joint-type dependent + // row 1: the upper bound + setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA)); - // row 1: the upper bound - setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA)); - - for (int row=0;row<getNumRows();row++) + for (int row = 0; row < getNumRows(); row++) { - - btScalar direction = row? -1 : 1; + btScalar penetration = getPosition(row); + + //todo: consider adding some safety threshold here + if (penetration > 0) + { + continue; + } + btScalar direction = row ? -1 : 1; btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); - constraintRow.m_orgConstraint = this; - constraintRow.m_orgDofIndex = row; - + constraintRow.m_orgConstraint = this; + constraintRow.m_orgDofIndex = row; + constraintRow.m_multiBodyA = m_bodyA; constraintRow.m_multiBodyB = m_bodyB; - const btScalar posError = 0; //why assume it's zero? + const btScalar posError = 0; //why assume it's zero? const btVector3 dummy(0, 0, 0); - btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse); + btScalar rel_vel = fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, 0, m_maxAppliedImpulse); { //expect either prismatic or revolute joint type for now - btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic)); + btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute) || (m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic)); switch (m_bodyA->getLink(m_linkA).m_jointType) { case btMultibodyLink::eRevolute: { constraintRow.m_contactNormal1.setZero(); constraintRow.m_contactNormal2.setZero(); - btVector3 revoluteAxisInWorld = direction*quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec); - constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld; - constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld; - + btVector3 revoluteAxisInWorld = direction * quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_topVec); + constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld; + constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld; + break; } case btMultibodyLink::ePrismatic: { - btVector3 prismaticAxisInWorld = direction* quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec); - constraintRow.m_contactNormal1=prismaticAxisInWorld; - constraintRow.m_contactNormal2=-prismaticAxisInWorld; + btVector3 prismaticAxisInWorld = direction * quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec); + constraintRow.m_contactNormal1 = prismaticAxisInWorld; + constraintRow.m_contactNormal2 = -prismaticAxisInWorld; constraintRow.m_relpos1CrossNormal.setZero(); constraintRow.m_relpos2CrossNormal.setZero(); - + break; } default: @@ -154,36 +158,35 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint btAssert(0); } }; - } { - btScalar penetration = getPosition(row); btScalar positionalError = 0.f; - btScalar velocityError = - rel_vel;// * damping; + btScalar velocityError = -rel_vel; // * damping; btScalar erp = infoGlobal.m_erp2; if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) { erp = infoGlobal.m_erp; } - if (penetration>0) + if (penetration > 0) { positionalError = 0; velocityError = -penetration / infoGlobal.m_timeStep; - } else + } + else { - positionalError = -penetration * erp/infoGlobal.m_timeStep; + positionalError = -penetration * erp / infoGlobal.m_timeStep; } - btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv; + btScalar penetrationImpulse = positionalError * constraintRow.m_jacDiagABInv; + btScalar velocityImpulse = velocityError * constraintRow.m_jacDiagABInv; if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) { //combine position and velocity into rhs - constraintRow.m_rhs = penetrationImpulse+velocityImpulse; + constraintRow.m_rhs = penetrationImpulse + velocityImpulse; constraintRow.m_rhsPenetration = 0.f; - - } else + } + else { //split position and velocity into rhs and m_rhsPenetration constraintRow.m_rhs = velocityImpulse; @@ -191,9 +194,4 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint } } } - } - - - - |