diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp')
-rw-r--r-- | extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp | 466 |
1 files changed, 157 insertions, 309 deletions
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index 44e04c3a132..12997d2e374 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -1,263 +1,101 @@ #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_num_rows(numRows), + m_numRows(numRows), + m_jacSizeA(0), + m_jacSizeBoth(0), m_isUnilateral(isUnilateral), + m_numDofsFinalized(-1), m_maxAppliedImpulse(100) { - m_jac_size_A = (6 + bodyA->getNumLinks()); - m_jac_size_both = (m_jac_size_A + (bodyB ? 6 + bodyB->getNumLinks() : 0)); - m_pos_offset = ((1 + m_jac_size_both)*m_num_rows); - m_data.resize((2 + m_jac_size_both) * m_num_rows); -} -btMultiBodyConstraint::~btMultiBodyConstraint() -{ } - - -btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow, - btMultiBodyJacobianData& data, - btScalar* jacOrgA,btScalar* jacOrgB, - const btContactSolverInfo& infoGlobal, - btScalar desiredVelocity, - btScalar lowerLimit, - btScalar upperLimit) +void btMultiBodyConstraint::updateJacobianSizes() { - - - - constraintRow.m_multiBodyA = m_bodyA; - constraintRow.m_multiBodyB = m_bodyB; - - btMultiBody* multiBodyA = constraintRow.m_multiBodyA; - btMultiBody* multiBodyB = constraintRow.m_multiBodyB; - - if (multiBodyA) + if(m_bodyA) { - - const int ndofA = multiBodyA->getNumLinks() + 6; - - constraintRow.m_deltaVelAindex = multiBodyA->getCompanionId(); - - if (constraintRow.m_deltaVelAindex <0) - { - constraintRow.m_deltaVelAindex = data.m_deltaVelocities.size(); - multiBodyA->setCompanionId(constraintRow.m_deltaVelAindex); - data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA); - } else - { - btAssert(data.m_deltaVelocities.size() >= constraintRow.m_deltaVelAindex+ndofA); - } - - constraintRow.m_jacAindex = data.m_jacobians.size(); - data.m_jacobians.resize(data.m_jacobians.size()+ndofA); - data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); - btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); - for (int i=0;i<ndofA;i++) - data.m_jacobians[constraintRow.m_jacAindex+i] = jacOrgA[i]; - - btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex]; - multiBodyA->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v); - } - - if (multiBodyB) - { - const int ndofB = multiBodyB->getNumLinks() + 6; - - constraintRow.m_deltaVelBindex = multiBodyB->getCompanionId(); - if (constraintRow.m_deltaVelBindex <0) - { - constraintRow.m_deltaVelBindex = data.m_deltaVelocities.size(); - multiBodyB->setCompanionId(constraintRow.m_deltaVelBindex); - data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB); - } - - constraintRow.m_jacBindex = data.m_jacobians.size(); - data.m_jacobians.resize(data.m_jacobians.size()+ndofB); - - for (int i=0;i<ndofB;i++) - data.m_jacobians[constraintRow.m_jacBindex+i] = jacOrgB[i]; - - data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB); - btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); - multiBodyB->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],data.scratch_r, data.scratch_v); - } - { - - btVector3 vec; - btScalar denom0 = 0.f; - btScalar denom1 = 0.f; - btScalar* jacB = 0; - btScalar* jacA = 0; - btScalar* lambdaA =0; - btScalar* lambdaB =0; - int ndofA = 0; - if (multiBodyA) - { - ndofA = multiBodyA->getNumLinks() + 6; - jacA = &data.m_jacobians[constraintRow.m_jacAindex]; - lambdaA = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex]; - for (int i = 0; i < ndofA; ++i) - { - btScalar j = jacA[i] ; - btScalar l =lambdaA[i]; - denom0 += j*l; - } - } - if (multiBodyB) - { - const int ndofB = multiBodyB->getNumLinks() + 6; - jacB = &data.m_jacobians[constraintRow.m_jacBindex]; - lambdaB = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex]; - for (int i = 0; i < ndofB; ++i) - { - btScalar j = jacB[i] ; - btScalar l =lambdaB[i]; - denom1 += j*l; - } - - } - - if (multiBodyA && (multiBodyA==multiBodyB)) - { - // ndof1 == ndof2 in this case - for (int i = 0; i < ndofA; ++i) - { - denom1 += jacB[i] * lambdaA[i]; - denom1 += jacA[i] * lambdaB[i]; - } - } - - btScalar d = denom0+denom1; - if (btFabs(d)>SIMD_EPSILON) - { - - constraintRow.m_jacDiagABInv = 1.f/(d); - } else - { - constraintRow.m_jacDiagABInv = 1.f; - } - + m_jacSizeA = (6 + m_bodyA->getNumDofs()); } - - //compute rhs and remaining constraintRow fields - - - - - btScalar rel_vel = 0.f; - int ndofA = 0; - int ndofB = 0; + if(m_bodyB) { + m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs(); + } + else + m_jacSizeBoth = m_jacSizeA; +} - btVector3 vel1,vel2; - if (multiBodyA) - { - ndofA = multiBodyA->getNumLinks() + 6; - btScalar* jacA = &data.m_jacobians[constraintRow.m_jacAindex]; - for (int i = 0; i < ndofA ; ++i) - rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; - } - if (multiBodyB) - { - ndofB = multiBodyB->getNumLinks() + 6; - btScalar* jacB = &data.m_jacobians[constraintRow.m_jacBindex]; - for (int i = 0; i < ndofB ; ++i) - rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; - - } - - constraintRow.m_friction = 0.f; - - constraintRow.m_appliedImpulse = 0.f; - constraintRow.m_appliedPushImpulse = 0.f; - - btScalar velocityError = desiredVelocity - rel_vel;// * damping; - - btScalar erp = infoGlobal.m_erp2; - - btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv; - - if (!infoGlobal.m_splitImpulse) - { - //combine position and velocity into rhs - constraintRow.m_rhs = velocityImpulse; - constraintRow.m_rhsPenetration = 0.f; - - } else - { - //split position and velocity into rhs and m_rhsPenetration - constraintRow.m_rhs = velocityImpulse; - constraintRow.m_rhsPenetration = 0.f; - } - - - constraintRow.m_cfm = 0.f; - constraintRow.m_lowerLimit = lowerLimit; - constraintRow.m_upperLimit = upperLimit; +void btMultiBodyConstraint::allocateJacobiansMultiDof() +{ + updateJacobianSizes(); - } - return rel_vel; + m_posOffset = ((1 + m_jacSizeBoth)*m_numRows); + m_data.resize((2 + m_jacSizeBoth) * m_numRows); } +btMultiBodyConstraint::~btMultiBodyConstraint() +{ +} void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof) { - for (int i = 0; i < ndof; ++i) + for (int i = 0; i < ndof; ++i) data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse; } - -void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstraint& solverConstraint, - btMultiBodyJacobianData& data, - const btVector3& contactNormalOnB, - const btVector3& posAworld, const btVector3& posBworld, - btScalar position, - const btContactSolverInfo& infoGlobal, - btScalar& relaxation, - bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) +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) { - - - btVector3 rel_pos1 = posAworld; - btVector3 rel_pos2 = posBworld; + solverConstraint.m_multiBodyA = m_bodyA; solverConstraint.m_multiBodyB = m_bodyB; solverConstraint.m_linkA = m_linkA; solverConstraint.m_linkB = m_linkB; - btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; - const btVector3& pos1 = posAworld; - const btVector3& pos2 = posBworld; - btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA); btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB); 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 if (bodyA) - rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); + rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin(); if (bodyB) - rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); - - relaxation = 1.f; + rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin(); if (multiBodyA) { - const int ndofA = multiBodyA->getNumLinks() + 6; + if (solverConstraint.m_linkA<0) + { + rel_pos1 = posAworld - multiBodyA->getBasePos(); + } else + { + rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin(); + } + + const int ndofA = multiBodyA->getNumDofs() + 6; solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); @@ -271,16 +109,35 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr 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_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); - btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); + //copy/determine + if(jacOrgA) + { + 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->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, 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 + btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size()); btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); - } else + //determine.. + multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v); + + btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB); + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + solverConstraint.m_contactNormal1 = contactNormalOnB; + } + else //if(rb0) { btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB); solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); @@ -290,7 +147,15 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr if (multiBodyB) { - const int ndofB = multiBodyB->getNumLinks() + 6; + if (solverConstraint.m_linkB<0) + { + rel_pos2 = posBworld - multiBodyB->getBasePos(); + } else + { + rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin(); + } + + const int ndofB = multiBodyB->getNumDofs() + 6; solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId(); if (solverConstraint.m_deltaVelBindex <0) @@ -300,144 +165,136 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr 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); + //copy/determine.. + if(jacOrgB) + { + 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); + } + + //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); 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->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m); - multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],data.scratch_r, data.scratch_v); - } else + btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB); + solverConstraint.m_relpos2CrossNormal = -torqueAxis1; + solverConstraint.m_contactNormal2 = -contactNormalOnB; + + } + else //if(rb1) { - btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB); + btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB); solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); solverConstraint.m_relpos2CrossNormal = -torqueAxis1; solverConstraint.m_contactNormal2 = -contactNormalOnB; } - { - + btVector3 vec; btScalar denom0 = 0.f; btScalar denom1 = 0.f; btScalar* jacB = 0; btScalar* jacA = 0; - btScalar* lambdaA =0; - btScalar* lambdaB =0; + btScalar* deltaVelA = 0; + btScalar* deltaVelB = 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) { - ndofA = multiBodyA->getNumLinks() + 6; + ndofA = multiBodyA->getNumDofs() + 6; jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; - lambdaA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; + deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; for (int i = 0; i < ndofA; ++i) { btScalar j = jacA[i] ; - btScalar l =lambdaA[i]; + btScalar l = deltaVelA[i]; denom0 += j*l; } - } else + } + else if(rb0) { - if (rb0) - { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec); - } + vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec); } + // if (multiBodyB) { - const int ndofB = multiBodyB->getNumLinks() + 6; + const int ndofB = multiBodyB->getNumDofs() + 6; jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; - lambdaB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; + deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; for (int i = 0; i < ndofB; ++i) { btScalar j = jacB[i] ; - btScalar l =lambdaB[i]; + btScalar l = deltaVelB[i]; denom1 += j*l; } - } else + } + else if(rb1) { - if (rb1) - { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec); - } + vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); + denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec); } - if (multiBodyA && (multiBodyA==multiBodyB)) - { - // ndof1 == ndof2 in this case - for (int i = 0; i < ndofA; ++i) - { - denom1 += jacB[i] * lambdaA[i]; - denom1 += jacA[i] * lambdaB[i]; - } - } - - btScalar d = denom0+denom1; - if (btFabs(d)>SIMD_EPSILON) - { - - solverConstraint.m_jacDiagABInv = relaxation/(d); - } else - { - solverConstraint.m_jacDiagABInv = 1.f; - } - + // + btScalar d = denom0+denom1; + if (d>SIMD_EPSILON) + { + solverConstraint.m_jacDiagABInv = relaxation/(d); + } + else + { + //disable the constraint row to handle singularity/redundant constraint + solverConstraint.m_jacDiagABInv = 0.f; + } } - - //compute rhs and remaining solverConstraint fields - - - btScalar restitution = 0.f; - btScalar penetration = isFriction? 0 : position+infoGlobal.m_linearSlop; + //compute rhs and remaining solverConstraint fields + btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop; btScalar rel_vel = 0.f; int ndofA = 0; int ndofB = 0; { - btVector3 vel1,vel2; if (multiBodyA) { - ndofA = multiBodyA->getNumLinks() + 6; + 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 + } + else if(rb0) { - if (rb0) - { - rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); - } + rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); } if (multiBodyB) { - ndofB = multiBodyB->getNumLinks() + 6; + 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 + } + else if(rb1) { - if (rb1) - { - rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); - } + rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); } solverConstraint.m_friction = 0.f;//cp.m_combinedFriction; - - - restitution = restitution * -rel_vel;//restitutionCurve(rel_vel, cp.m_combinedRestitution); - if (restitution <= btScalar(0.)) - { - restitution = 0.f; - }; } @@ -474,18 +331,15 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr } } else */ - { - solverConstraint.m_appliedImpulse = 0.f; - } + solverConstraint.m_appliedImpulse = 0.f; solverConstraint.m_appliedPushImpulse = 0.f; { - btScalar positionalError = 0.f; - btScalar velocityError = restitution - rel_vel;// * damping; - + btScalar velocityError = desiredVelocity - rel_vel;// * damping; + btScalar erp = infoGlobal.m_erp2; if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) @@ -493,15 +347,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr erp = infoGlobal.m_erp; } - if (penetration>0) - { - positionalError = 0; - velocityError = -penetration / infoGlobal.m_timeStep; - - } else - { - positionalError = -penetration * erp/infoGlobal.m_timeStep; - } + positionalError = -penetration * erp/infoGlobal.m_timeStep; btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; @@ -520,8 +366,10 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr } solverConstraint.m_cfm = 0.f; - solverConstraint.m_lowerLimit = -m_maxAppliedImpulse; - solverConstraint.m_upperLimit = m_maxAppliedImpulse; + solverConstraint.m_lowerLimit = lowerLimit; + solverConstraint.m_upperLimit = upperLimit; } + return rel_vel; + } |