diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp')
| -rw-r--r-- | extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp | 139 |
1 files changed, 67 insertions, 72 deletions
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp index 12b21f74603..f51e69deb1c 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp @@ -21,29 +21,29 @@ subject to the following restrictions: #include "LinearMath/btIDebugDraw.h" #ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST - #define BTMBP2PCONSTRAINT_DIM 3 +#define BTMBP2PCONSTRAINT_DIM 3 #else - #define BTMBP2PCONSTRAINT_DIM 6 +#define BTMBP2PCONSTRAINT_DIM 6 #endif btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB) - :btMultiBodyConstraint(body,0,link,-1,BTMBP2PCONSTRAINT_DIM,false), - m_rigidBodyA(0), - m_rigidBodyB(bodyB), - m_pivotInA(pivotInA), - m_pivotInB(pivotInB) + : btMultiBodyConstraint(body, 0, link, -1, BTMBP2PCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_POINT_TO_POINT), + m_rigidBodyA(0), + m_rigidBodyB(bodyB), + m_pivotInA(pivotInA), + m_pivotInB(pivotInB) { - m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses + m_data.resize(BTMBP2PCONSTRAINT_DIM); //at least store the applied impulses } btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB) - :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBP2PCONSTRAINT_DIM,false), - m_rigidBodyA(0), - m_rigidBodyB(0), - m_pivotInA(pivotInA), - m_pivotInB(pivotInB) + : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBP2PCONSTRAINT_DIM, false, MULTIBODY_CONSTRAINT_POINT_TO_POINT), + m_rigidBodyA(0), + m_rigidBodyB(0), + m_pivotInA(pivotInA), + m_pivotInB(pivotInB) { - m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses + m_data.resize(BTMBP2PCONSTRAINT_DIM); //at least store the applied impulses } void btMultiBodyPoint2Point::finalizeMultiDof() @@ -56,7 +56,6 @@ btMultiBodyPoint2Point::~btMultiBodyPoint2Point() { } - int btMultiBodyPoint2Point::getIslandIdA() const { if (m_rigidBodyA) @@ -64,13 +63,16 @@ int btMultiBodyPoint2Point::getIslandIdA() const 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; @@ -82,62 +84,58 @@ int btMultiBodyPoint2Point::getIslandIdB() const return m_rigidBodyB->getIslandTag(); 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 btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows, - btMultiBodyJacobianData& data, - const btContactSolverInfo& infoGlobal) + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal) { - -// int i=1; -int numDim = BTMBP2PCONSTRAINT_DIM; - for (int i=0;i<numDim;i++) + // int i=1; + int numDim = BTMBP2PCONSTRAINT_DIM; + for (int i = 0; i < numDim; i++) { - btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); - //memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint)); - 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); + //memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint)); + 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; - btVector3 contactNormalOnB(0,0,0); + btVector3 contactNormalOnB(0, 0, 0); #ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST contactNormalOnB[i] = -1; #else - contactNormalOnB[i%3] = -1; + contactNormalOnB[i % 3] = -1; #endif - - // Convert local points back to world + // Convert local points back to world btVector3 pivotAworld = m_pivotInA; if (m_rigidBodyA) { - constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId(); - pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA; - } else + pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA; + } + else { if (m_bodyA) pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); @@ -146,44 +144,41 @@ int numDim = BTMBP2PCONSTRAINT_DIM; if (m_rigidBodyB) { constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId(); - pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB; - } else + pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB; + } + else { if (m_bodyB) pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); - } - btScalar posError = i < 3 ? (pivotAworld-pivotBworld).dot(contactNormalOnB) : 0; + btScalar posError = i < 3 ? (pivotAworld - pivotBworld).dot(contactNormalOnB) : 0; #ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST - - fillMultiBodyConstraint(constraintRow, data, 0, 0, - contactNormalOnB, pivotAworld, pivotBworld, //sucks but let it be this way "for the time being" - posError, - infoGlobal, - -m_maxAppliedImpulse, m_maxAppliedImpulse - ); - //@todo: support the case of btMultiBody versus btRigidBody, - //see btPoint2PointConstraint::getInfo2NonVirtual + fillMultiBodyConstraint(constraintRow, data, 0, 0, btVector3(0, 0, 0), + contactNormalOnB, pivotAworld, pivotBworld, //sucks but let it be this way "for the time being" + posError, + infoGlobal, + -m_maxAppliedImpulse, m_maxAppliedImpulse); + //@todo: support the case of btMultiBody versus btRigidBody, + //see btPoint2PointConstraint::getInfo2NonVirtual #else const btVector3 dummy(0, 0, 0); btAssert(m_bodyA->isMultiDof()); btScalar* jac1 = jacobianA(i); - const btVector3 &normalAng = i >= 3 ? contactNormalOnB : dummy; - const btVector3 &normalLin = i < 3 ? contactNormalOnB : dummy; + const btVector3& normalAng = i >= 3 ? contactNormalOnB : dummy; + const btVector3& normalLin = i < 3 ? contactNormalOnB : dummy; m_bodyA->filConstraintJacobianMultiDof(m_linkA, pivotAworld, normalAng, normalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m); fillMultiBodyConstraint(constraintRow, data, jac1, 0, - dummy, dummy, dummy, //sucks but let it be this way "for the time being" - posError, - infoGlobal, - -m_maxAppliedImpulse, m_maxAppliedImpulse - ); + dummy, dummy, dummy, //sucks but let it be this way "for the time being" + posError, + infoGlobal, + -m_maxAppliedImpulse, m_maxAppliedImpulse); #endif } } |