Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Code-style consistency improvement:

Browse Source 
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files.
make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type.
This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans
2018-09-23 14:17:31 -07:00
parent b73b05e9fb
commit ab8f16961e
1773 changed files with 1081087 additions and 474249 deletions
Download Patch File Download Diff File Expand all files Collapse all files

86
src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp
View File

@@ -21,20 +21,18 @@ subject to the following restrictions:
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
btMultiBodyGearConstraint::btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,1,false),
m_gearRatio(1),
m_gearAuxLink(-1),
m_erp(0),
m_relativePositionTarget(0)
: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, 1, false),
m_gearRatio(1),
m_gearAuxLink(-1),
m_erp(0),
m_relativePositionTarget(0)
{
}
void btMultiBodyGearConstraint::finalizeMultiDof()
{
allocateJacobiansMultiDof();
m_numDofsFinalized = m_jacSizeBoth;
}
@@ -42,7 +40,6 @@ btMultiBodyGearConstraint::~btMultiBodyGearConstraint()
{
}
int btMultiBodyGearConstraint::getIslandIdA() const
{
if (m_bodyA)
@@ -81,27 +78,25 @@ int btMultiBodyGearConstraint::getIslandIdB() const
return -1;
}
void btMultiBodyGearConstraint::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();
}
//don't crash
if (m_numDofsFinalized != m_jacSizeBoth)
return;
if (m_maxAppliedImpulse==0.f)
if (m_maxAppliedImpulse == 0.f)
return;
// note: we rely on the fact that data.m_jacobians are
// always initialized to zero by the Constraint ctor
int linkDoF = 0;
@@ -114,67 +109,66 @@ void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray&
btScalar posError = 0;
const btVector3 dummy(0, 0, 0);
btScalar kp = 1;
btScalar kd = 1;
int numRows = getNumRows();
for (int row=0;row<numRows;row++)
for (int row = 0; row < numRows; row++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
int dof = 0;
btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
int dof = 0;
btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
btScalar auxVel = 0;
if (m_gearAuxLink>=0)
if (m_gearAuxLink >= 0)
{
auxVel = m_bodyA->getJointVelMultiDof(m_gearAuxLink)[dof];
}
currentVelocity += auxVel;
if (m_erp!=0)
if (m_erp != 0)
{
btScalar currentPositionA = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
if (m_gearAuxLink >= 0)
{
currentPositionA -= m_bodyA->getJointPosMultiDof(m_gearAuxLink)[dof];
}
btScalar currentPositionB = m_gearRatio*m_bodyA->getJointPosMultiDof(m_linkB)[dof];
btScalar diff = currentPositionB+currentPositionA;
btScalar currentPositionB = m_gearRatio * m_bodyA->getJointPosMultiDof(m_linkB)[dof];
btScalar diff = currentPositionB + currentPositionA;
btScalar desiredPositionDiff = this->m_relativePositionTarget;
posError = -m_erp*(desiredPositionDiff - diff);
posError = -m_erp * (desiredPositionDiff - diff);
}
btScalar desiredRelativeVelocity = auxVel;
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,desiredRelativeVelocity);
btScalar desiredRelativeVelocity = auxVel;
fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, -m_maxAppliedImpulse, m_maxAppliedImpulse, false, 1, false, desiredRelativeVelocity);
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = row;
{
//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 = 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 = 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 = 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 = 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();
constraintRow.m_relpos2CrossNormal.setZero();
break;
}
default:
@@ -182,10 +176,6 @@ void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray&
btAssert(0);
}
};
}
}
}