diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp')
| -rw-r--r-- | thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp | 184 |
1 files changed, 92 insertions, 92 deletions
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp index af48e94a83..5ef9444c2f 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp @@ -24,27 +24,27 @@ subject to the following restrictions: #define BTMBFIXEDCONSTRAINT_DIM 6 btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB) - :btMultiBodyConstraint(body,0,link,-1,BTMBFIXEDCONSTRAINT_DIM,false), - m_rigidBodyA(0), - m_rigidBodyB(bodyB), - m_pivotInA(pivotInA), - m_pivotInB(pivotInB), - m_frameInA(frameInA), - m_frameInB(frameInB) + : btMultiBodyConstraint(body, 0, link, -1, BTMBFIXEDCONSTRAINT_DIM, false), + m_rigidBodyA(0), + m_rigidBodyB(bodyB), + m_pivotInA(pivotInA), + m_pivotInB(pivotInB), + m_frameInA(frameInA), + m_frameInB(frameInB) { - m_data.resize(BTMBFIXEDCONSTRAINT_DIM);//at least store the applied impulses + m_data.resize(BTMBFIXEDCONSTRAINT_DIM); //at least store the applied impulses } btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(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,BTMBFIXEDCONSTRAINT_DIM,false), - m_rigidBodyA(0), - m_rigidBodyB(0), - m_pivotInA(pivotInA), - m_pivotInB(pivotInB), - m_frameInA(frameInA), - m_frameInB(frameInB) + : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBFIXEDCONSTRAINT_DIM, false), + m_rigidBodyA(0), + m_rigidBodyB(0), + m_pivotInA(pivotInA), + m_pivotInB(pivotInB), + m_frameInA(frameInA), + m_frameInB(frameInB) { - m_data.resize(BTMBFIXEDCONSTRAINT_DIM);//at least store the applied impulses + m_data.resize(BTMBFIXEDCONSTRAINT_DIM); //at least store the applied impulses } void btMultiBodyFixedConstraint::finalizeMultiDof() @@ -57,7 +57,6 @@ btMultiBodyFixedConstraint::~btMultiBodyFixedConstraint() { } - int btMultiBodyFixedConstraint::getIslandIdA() const { if (m_rigidBodyA) @@ -103,82 +102,83 @@ int btMultiBodyFixedConstraint::getIslandIdB() const void btMultiBodyFixedConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal) { - int numDim = BTMBFIXEDCONSTRAINT_DIM; - for (int i=0;i<numDim;i++) + int numDim = BTMBFIXEDCONSTRAINT_DIM; + for (int i = 0; i < numDim; i++) { - btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); - constraintRow.m_orgConstraint = this; - constraintRow.m_orgDofIndex = i; - constraintRow.m_relpos1CrossNormal.setValue(0,0,0); - constraintRow.m_contactNormal1.setValue(0,0,0); - constraintRow.m_relpos2CrossNormal.setValue(0,0,0); - constraintRow.m_contactNormal2.setValue(0,0,0); - constraintRow.m_angularComponentA.setValue(0,0,0); - constraintRow.m_angularComponentB.setValue(0,0,0); - - constraintRow.m_solverBodyIdA = data.m_fixedBodyId; - constraintRow.m_solverBodyIdB = data.m_fixedBodyId; - - // Convert local points back to world - btVector3 pivotAworld = m_pivotInA; - btMatrix3x3 frameAworld = m_frameInA; - if (m_rigidBodyA) - { - - constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId(); - pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA; - frameAworld = frameAworld.transpose()*btMatrix3x3(m_rigidBodyA->getOrientation()); - - } else - { - if (m_bodyA) { - pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); - frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld); - } - } - btVector3 pivotBworld = m_pivotInB; - btMatrix3x3 frameBworld = m_frameInB; - if (m_rigidBodyB) - { - constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId(); - pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB; - frameBworld = frameBworld.transpose()*btMatrix3x3(m_rigidBodyB->getOrientation()); - - } else - { - if (m_bodyB) { - pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); - frameBworld = m_bodyB->localFrameToWorld(m_linkB, frameBworld); - } - } - - btMatrix3x3 relRot = frameAworld.inverse()*frameBworld; - btVector3 angleDiff; - btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot,angleDiff); - - btVector3 constraintNormalLin(0,0,0); - btVector3 constraintNormalAng(0,0,0); - btScalar posError = 0.0; - if (i < 3) { - constraintNormalLin[i] = 1; - posError = (pivotAworld-pivotBworld).dot(constraintNormalLin); - fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng, - constraintNormalLin, pivotAworld, pivotBworld, - posError, - infoGlobal, - -m_maxAppliedImpulse, m_maxAppliedImpulse - ); - } - else { //i>=3 - constraintNormalAng = frameAworld.getColumn(i%3); - posError = angleDiff[i%3]; - fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng, - constraintNormalLin, pivotAworld, pivotBworld, - posError, - infoGlobal, - -m_maxAppliedImpulse, m_maxAppliedImpulse, true - ); - } + btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); + constraintRow.m_orgConstraint = this; + constraintRow.m_orgDofIndex = i; + constraintRow.m_relpos1CrossNormal.setValue(0, 0, 0); + constraintRow.m_contactNormal1.setValue(0, 0, 0); + constraintRow.m_relpos2CrossNormal.setValue(0, 0, 0); + constraintRow.m_contactNormal2.setValue(0, 0, 0); + constraintRow.m_angularComponentA.setValue(0, 0, 0); + constraintRow.m_angularComponentB.setValue(0, 0, 0); + + constraintRow.m_solverBodyIdA = data.m_fixedBodyId; + constraintRow.m_solverBodyIdB = data.m_fixedBodyId; + + // Convert local points back to world + btVector3 pivotAworld = m_pivotInA; + btMatrix3x3 frameAworld = m_frameInA; + if (m_rigidBodyA) + { + constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId(); + pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA; + frameAworld = frameAworld.transpose() * btMatrix3x3(m_rigidBodyA->getOrientation()); + } + else + { + if (m_bodyA) + { + pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); + frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld); + } + } + btVector3 pivotBworld = m_pivotInB; + btMatrix3x3 frameBworld = m_frameInB; + if (m_rigidBodyB) + { + constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId(); + pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB; + frameBworld = frameBworld.transpose() * btMatrix3x3(m_rigidBodyB->getOrientation()); + } + else + { + if (m_bodyB) + { + pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); + frameBworld = m_bodyB->localFrameToWorld(m_linkB, frameBworld); + } + } + + btMatrix3x3 relRot = frameAworld.inverse() * frameBworld; + btVector3 angleDiff; + btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot, angleDiff); + + btVector3 constraintNormalLin(0, 0, 0); + btVector3 constraintNormalAng(0, 0, 0); + btScalar posError = 0.0; + if (i < 3) + { + constraintNormalLin[i] = 1; + posError = (pivotAworld - pivotBworld).dot(constraintNormalLin); + fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng, + constraintNormalLin, pivotAworld, pivotBworld, + posError, + infoGlobal, + -m_maxAppliedImpulse, m_maxAppliedImpulse); + } + else + { //i>=3 + constraintNormalAng = frameAworld.getColumn(i % 3); + posError = angleDiff[i % 3]; + fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng, + constraintNormalLin, pivotAworld, pivotBworld, + posError, + infoGlobal, + -m_maxAppliedImpulse, m_maxAppliedImpulse, true); + } } } |