diff options
author | Brecht Van Lommel <brechtvanlommel@gmail.com> | 2016-01-17 23:35:32 +0300 |
---|---|---|
committer | Brecht Van Lommel <brechtvanlommel@gmail.com> | 2016-01-26 00:14:46 +0300 |
commit | b64d5809e7e3b832e2a011869db68e70b4b4e6fc (patch) | |
tree | aa4f6714da9f546eeee7dffed9236f9c8309524b /extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp | |
parent | 3c72e302e1eb25de43dd9d077f0c730cc02b5674 (diff) |
Upgrade Bullet to version 2.83.
I tried to carefully preserve all patches since the last upgrade.
Improves T47195, cloth collision detection bug.
Differential Revision: https://developer.blender.org/D1739
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp')
-rw-r--r-- | extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp | 98 |
1 files changed, 83 insertions, 15 deletions
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp index ab5a430231b..062d19accaa 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -4,8 +4,8 @@ Copyright (c) 2013 Erwin Coumans http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. @@ -22,18 +22,41 @@ 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) + :btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true), + m_desiredVelocity(desiredVelocity) { + m_maxAppliedImpulse = maxMotorImpulse; // the data.m_jacobians never change, so may as well // initialize them here - - // note: we rely on the fact that data.m_jacobians are - // always initialized to zero by the Constraint ctor - // row 0: the lower bound - jacobianA(0)[6 + link] = 1; + +} + +void btMultiBodyJointMotor::finalizeMultiDof() +{ + allocateJacobiansMultiDof(); + // note: we rely on the fact that data.m_jacobians are + // always initialized to zero by the Constraint ctor + int linkDoF = 0; + unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF); + + // row 0: the lower bound + // row 0: the lower bound + jacobianA(0)[offset] = 1; + + m_numDofsFinalized = m_jacSizeBoth; +} + +btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse) + //:btMultiBodyConstraint(body,0,link,-1,1,true), + :btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true), + m_desiredVelocity(desiredVelocity) +{ + btAssert(linkDoF < body->getLink(link).m_dofCount); + + m_maxAppliedImpulse = maxMotorImpulse; + } btMultiBodyJointMotor::~btMultiBodyJointMotor() { @@ -74,16 +97,61 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con { // 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(); + } + + //don't crash + if (m_numDofsFinalized != m_jacSizeBoth) + return; + + const btScalar posError = 0; + const btVector3 dummy(0, 0, 0); for (int row=0;row<getNumRows();row++) { btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); - - btScalar penetration = 0; - fillConstraintRowMultiBodyMultiBody(constraintRow,data,jacobianA(row),jacobianB(row),infoGlobal,m_desiredVelocity,-m_maxAppliedImpulse,m_maxAppliedImpulse); + + + fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity); + 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)); + 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; + + 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; + constraintRow.m_relpos1CrossNormal.setZero(); + constraintRow.m_relpos2CrossNormal.setZero(); + + break; + } + default: + { + btAssert(0); + } + }; + + } + } } - + |