summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp')
-rw-r--r--thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp94
1 files changed, 40 insertions, 54 deletions
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
index 35c929f7ce..8791ad2868 100644
--- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
+++ b/thirdparty/bullet/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),
+ 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();
@@ -53,10 +50,8 @@ btMultiBodyJointLimitConstraint::~btMultiBodyJointLimitConstraint()
{
}
-
int btMultiBodyJointLimitConstraint::getIslandIdA() const
{
-
if (m_bodyA)
{
if (m_linkA < 0)
@@ -93,72 +88,69 @@ int btMultiBodyJointLimitConstraint::getIslandIdB() const
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 penetration = getPosition(row);
//todo: consider adding some safety threshold here
- if (penetration>0)
+ if (penetration > 0)
{
continue;
}
- btScalar direction = row? -1 : 1;
+ 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,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:
@@ -166,36 +158,35 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
btAssert(0);
}
};
-
}
{
-
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;
@@ -203,9 +194,4 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
}
}
}
-
}
-
-
-
-
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-13 03:11:23 +0000