diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp')
-rw-r--r-- | extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp | 325 |
1 files changed, 169 insertions, 156 deletions
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index 12997d2e374..1ba5861145b 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -1,32 +1,30 @@ #include "btMultiBodyConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" -#include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro) - - - -btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral) - :m_bodyA(bodyA), - m_bodyB(bodyB), - m_linkA(linkA), - m_linkB(linkB), - m_numRows(numRows), - m_jacSizeA(0), - m_jacSizeBoth(0), - m_isUnilateral(isUnilateral), - m_numDofsFinalized(-1), - m_maxAppliedImpulse(100) +#include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro) + +btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA, btMultiBody* bodyB, int linkA, int linkB, int numRows, bool isUnilateral, int type) + : m_bodyA(bodyA), + m_bodyB(bodyB), + m_linkA(linkA), + m_linkB(linkB), + m_type(type), + m_numRows(numRows), + m_jacSizeA(0), + m_jacSizeBoth(0), + m_isUnilateral(isUnilateral), + m_numDofsFinalized(-1), + m_maxAppliedImpulse(100) { - } void btMultiBodyConstraint::updateJacobianSizes() { - if(m_bodyA) + if (m_bodyA) { m_jacSizeA = (6 + m_bodyA->getNumDofs()); } - if(m_bodyB) + if (m_bodyB) { m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs(); } @@ -38,7 +36,7 @@ void btMultiBodyConstraint::allocateJacobiansMultiDof() { updateJacobianSizes(); - m_posOffset = ((1 + m_jacSizeBoth)*m_numRows); + m_posOffset = ((1 + m_jacSizeBoth) * m_numRows); m_data.resize((2 + m_jacSizeBoth) * m_numRows); } @@ -46,25 +44,25 @@ btMultiBodyConstraint::~btMultiBodyConstraint() { } -void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) +void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) { for (int i = 0; i < ndof; ++i) - data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse; + data.m_deltaVelocities[velocityIndex + i] += delta_vee[i] * impulse; } -btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint, - btMultiBodyJacobianData& data, - btScalar* jacOrgA, btScalar* jacOrgB, - const btVector3& contactNormalOnB, - const btVector3& posAworld, const btVector3& posBworld, - btScalar posError, - const btContactSolverInfo& infoGlobal, - btScalar lowerLimit, btScalar upperLimit, - btScalar relaxation, - bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) +btScalar btMultiBodyConstraint::fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint, + btMultiBodyJacobianData& data, + btScalar* jacOrgA, btScalar* jacOrgB, + const btVector3& constraintNormalAng, + const btVector3& constraintNormalLin, + const btVector3& posAworld, const btVector3& posBworld, + btScalar posError, + const btContactSolverInfo& infoGlobal, + btScalar lowerLimit, btScalar upperLimit, + bool angConstraint, + btScalar relaxation, + bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) { - - solverConstraint.m_multiBodyA = m_bodyA; solverConstraint.m_multiBodyB = m_bodyB; solverConstraint.m_linkA = m_linkA; @@ -79,7 +77,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; - btVector3 rel_pos1, rel_pos2; //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary + btVector3 rel_pos1, rel_pos2; //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary if (bodyA) rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin(); if (bodyB) @@ -87,121 +85,156 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr if (multiBodyA) { - if (solverConstraint.m_linkA<0) + if (solverConstraint.m_linkA < 0) { rel_pos1 = posAworld - multiBodyA->getBasePos(); - } else + } + else { rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin(); } - const int ndofA = multiBodyA->getNumDofs() + 6; + const int ndofA = multiBodyA->getNumDofs() + 6; solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); - if (solverConstraint.m_deltaVelAindex <0) + if (solverConstraint.m_deltaVelAindex < 0) { solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size(); multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex); - data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA); - } else + data.m_deltaVelocities.resize(data.m_deltaVelocities.size() + ndofA); + } + else { - btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA); + btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex + ndofA); } //determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom //resize.. solverConstraint.m_jacAindex = data.m_jacobians.size(); - data.m_jacobians.resize(data.m_jacobians.size()+ndofA); + data.m_jacobians.resize(data.m_jacobians.size() + ndofA); //copy/determine - if(jacOrgA) + if (jacOrgA) { - for (int i=0;i<ndofA;i++) - data.m_jacobians[solverConstraint.m_jacAindex+i] = jacOrgA[i]; + for (int i = 0; i < ndofA; i++) + data.m_jacobians[solverConstraint.m_jacAindex + i] = jacOrgA[i]; } else { - btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex]; - multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m); + btScalar* jac1 = &data.m_jacobians[solverConstraint.m_jacAindex]; + //multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m); + multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalAng, constraintNormalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m); } //determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint) //resize.. - data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse + data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size() + ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; //determine.. - multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); + multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex], delta, data.scratch_r, data.scratch_v); - btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB); + btVector3 torqueAxis0; + if (angConstraint) + { + torqueAxis0 = constraintNormalAng; + } + else + { + torqueAxis0 = rel_pos1.cross(constraintNormalLin); + } solverConstraint.m_relpos1CrossNormal = torqueAxis0; - solverConstraint.m_contactNormal1 = contactNormalOnB; + solverConstraint.m_contactNormal1 = constraintNormalLin; } - else //if(rb0) + else //if(rb0) { - btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB); - solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); + btVector3 torqueAxis0; + if (angConstraint) + { + torqueAxis0 = constraintNormalAng; + } + else + { + torqueAxis0 = rel_pos1.cross(constraintNormalLin); + } + solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0); solverConstraint.m_relpos1CrossNormal = torqueAxis0; - solverConstraint.m_contactNormal1 = contactNormalOnB; + solverConstraint.m_contactNormal1 = constraintNormalLin; } if (multiBodyB) { - if (solverConstraint.m_linkB<0) + if (solverConstraint.m_linkB < 0) { rel_pos2 = posBworld - multiBodyB->getBasePos(); - } else + } + else { rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin(); } - const int ndofB = multiBodyB->getNumDofs() + 6; + const int ndofB = multiBodyB->getNumDofs() + 6; solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); - if (solverConstraint.m_deltaVelBindex <0) + if (solverConstraint.m_deltaVelBindex < 0) { solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size(); multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); - data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB); + data.m_deltaVelocities.resize(data.m_deltaVelocities.size() + ndofB); } //determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom //resize.. solverConstraint.m_jacBindex = data.m_jacobians.size(); - data.m_jacobians.resize(data.m_jacobians.size()+ndofB); + data.m_jacobians.resize(data.m_jacobians.size() + ndofB); //copy/determine.. - if(jacOrgB) + if (jacOrgB) { - for (int i=0;i<ndofB;i++) - data.m_jacobians[solverConstraint.m_jacBindex+i] = jacOrgB[i]; + for (int i = 0; i < ndofB; i++) + data.m_jacobians[solverConstraint.m_jacBindex + i] = jacOrgB[i]; } else { - multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); + //multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); + multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalAng, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); } //determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint) //resize.. - data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB); + data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size() + ndofB); btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; //determine.. - multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v); + multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex], delta, data.scratch_r, data.scratch_v); - btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB); + btVector3 torqueAxis1; + if (angConstraint) + { + torqueAxis1 = constraintNormalAng; + } + else + { + torqueAxis1 = rel_pos2.cross(constraintNormalLin); + } solverConstraint.m_relpos2CrossNormal = -torqueAxis1; - solverConstraint.m_contactNormal2 = -contactNormalOnB; - + solverConstraint.m_contactNormal2 = -constraintNormalLin; } - else //if(rb1) + else //if(rb1) { - btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB); - solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); + btVector3 torqueAxis1; + if (angConstraint) + { + torqueAxis1 = constraintNormalAng; + } + else + { + torqueAxis1 = rel_pos2.cross(constraintNormalLin); + } + solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * -torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0); solverConstraint.m_relpos2CrossNormal = -torqueAxis1; - solverConstraint.m_contactNormal2 = -contactNormalOnB; + solverConstraint.m_contactNormal2 = -constraintNormalLin; } { - btVector3 vec; btScalar denom0 = 0.f; btScalar denom1 = 0.f; @@ -209,7 +242,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr btScalar* jacA = 0; btScalar* deltaVelA = 0; btScalar* deltaVelB = 0; - int ndofA = 0; + int ndofA = 0; //determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i]) if (multiBodyA) { @@ -218,15 +251,22 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; for (int i = 0; i < ndofA; ++i) { - btScalar j = jacA[i] ; + btScalar j = jacA[i]; btScalar l = deltaVelA[i]; - denom0 += j*l; + denom0 += j * l; } } - else if(rb0) + else if (rb0) { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec); + vec = (solverConstraint.m_angularComponentA).cross(rel_pos1); + if (angConstraint) + { + denom0 = constraintNormalAng.dot(solverConstraint.m_angularComponentA); + } + else + { + denom0 = rb0->getInvMass() + constraintNormalLin.dot(vec); + } } // if (multiBodyB) @@ -236,134 +276,108 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; for (int i = 0; i < ndofB; ++i) { - btScalar j = jacB[i] ; + btScalar j = jacB[i]; btScalar l = deltaVelB[i]; - denom1 += j*l; + denom1 += j * l; } - } - else if(rb1) + else if (rb1) { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec); + vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2); + if (angConstraint) + { + denom1 = constraintNormalAng.dot(-solverConstraint.m_angularComponentB); + } + else + { + denom1 = rb1->getInvMass() + constraintNormalLin.dot(vec); + } } // - btScalar d = denom0+denom1; - if (d>SIMD_EPSILON) + btScalar d = denom0 + denom1; + if (d > SIMD_EPSILON) { - solverConstraint.m_jacDiagABInv = relaxation/(d); + solverConstraint.m_jacDiagABInv = relaxation / (d); } else { - //disable the constraint row to handle singularity/redundant constraint - solverConstraint.m_jacDiagABInv = 0.f; + //disable the constraint row to handle singularity/redundant constraint + solverConstraint.m_jacDiagABInv = 0.f; } } - //compute rhs and remaining solverConstraint fields - btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop; + btScalar penetration = isFriction ? 0 : posError; btScalar rel_vel = 0.f; - int ndofA = 0; - int ndofB = 0; + int ndofA = 0; + int ndofB = 0; { - btVector3 vel1,vel2; + btVector3 vel1, vel2; if (multiBodyA) { ndofA = multiBodyA->getNumDofs() + 6; btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA ; ++i) + for (int i = 0; i < ndofA; ++i) rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; } - else if(rb0) + else if (rb0) { - rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); + rel_vel += rb0->getLinearVelocity().dot(solverConstraint.m_contactNormal1); + rel_vel += rb0->getAngularVelocity().dot(solverConstraint.m_relpos1CrossNormal); } if (multiBodyB) { ndofB = multiBodyB->getNumDofs() + 6; btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; - for (int i = 0; i < ndofB ; ++i) + for (int i = 0; i < ndofB; ++i) rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; - } - else if(rb1) + else if (rb1) { - rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); + rel_vel += rb1->getLinearVelocity().dot(solverConstraint.m_contactNormal2); + rel_vel += rb1->getAngularVelocity().dot(solverConstraint.m_relpos2CrossNormal); } - solverConstraint.m_friction = 0.f;//cp.m_combinedFriction; + solverConstraint.m_friction = 0.f; //cp.m_combinedFriction; } - - ///warm starting (or zero if disabled) - /* - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; - - if (solverConstraint.m_appliedImpulse) - { - if (multiBodyA) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->applyDeltaVee(deltaV,impulse); - applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA); - } else - { - if (rb0) - bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); - } - if (multiBodyB) - { - btScalar impulse = solverConstraint.m_appliedImpulse; - btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - multiBodyB->applyDeltaVee(deltaV,impulse); - applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB); - } else - { - if (rb1) - bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); - } - } - } else - */ - solverConstraint.m_appliedImpulse = 0.f; solverConstraint.m_appliedPushImpulse = 0.f; { - btScalar positionalError = 0.f; - btScalar velocityError = desiredVelocity - rel_vel;// * damping; - + btScalar velocityError = desiredVelocity - rel_vel; // * damping; btScalar erp = infoGlobal.m_erp2; - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + + //split impulse is not implemented yet for btMultiBody* + //if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) { erp = infoGlobal.m_erp; } - positionalError = -penetration * erp/infoGlobal.m_timeStep; + positionalError = -penetration * erp / infoGlobal.m_timeStep; - btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; + btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + //split impulse is not implemented yet for btMultiBody* + + // if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) { //combine position and velocity into rhs - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; solverConstraint.m_rhsPenetration = 0.f; - - } else - { - //split position and velocity into rhs and m_rhsPenetration - solverConstraint.m_rhs = velocityImpulse; - solverConstraint.m_rhsPenetration = penetrationImpulse; } + /*else + { + //split position and velocity into rhs and m_rhsPenetration + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhsPenetration = penetrationImpulse; + } + */ solverConstraint.m_cfm = 0.f; solverConstraint.m_lowerLimit = lowerLimit; @@ -371,5 +385,4 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr } return rel_vel; - } |