diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp')
| -rw-r--r-- | thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp | 389 |
1 files changed, 0 insertions, 389 deletions
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp deleted file mode 100644 index 00d5fd5609..0000000000 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ /dev/null @@ -1,389 +0,0 @@ -#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, 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) - { - m_jacSizeA = (6 + m_bodyA->getNumDofs()); - } - - if (m_bodyB) - { - m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs(); - } - else - m_jacSizeBoth = m_jacSizeA; -} - -void btMultiBodyConstraint::allocateJacobiansMultiDof() -{ - updateJacobianSizes(); - - 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) - data.m_deltaVelocities[velocityIndex + i] += delta_vee[i] * impulse; -} - -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, - btScalar damping) -{ - 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; - - 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 = posAworld - bodyA->getWorldTransform().getOrigin(); - if (bodyB) - rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin(); - - if (multiBodyA) - { - 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(); - - 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 - { - 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); - //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, 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 - 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); - - btVector3 torqueAxis0; - if (angConstraint) - { - torqueAxis0 = constraintNormalAng; - } - else - { - torqueAxis0 = rel_pos1.cross(constraintNormalLin); - } - solverConstraint.m_relpos1CrossNormal = torqueAxis0; - solverConstraint.m_contactNormal1 = constraintNormalLin; - } - else //if(rb0) - { - 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 = constraintNormalLin; - } - - if (multiBodyB) - { - 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) - { - solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size(); - multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex); - 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, -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); - 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); - - btVector3 torqueAxis1; - if (angConstraint) - { - torqueAxis1 = constraintNormalAng; - } - else - { - torqueAxis1 = rel_pos2.cross(constraintNormalLin); - } - solverConstraint.m_relpos2CrossNormal = -torqueAxis1; - solverConstraint.m_contactNormal2 = -constraintNormalLin; - } - else //if(rb1) - { - 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 = -constraintNormalLin; - } - { - btVector3 vec; - btScalar denom0 = 0.f; - btScalar denom1 = 0.f; - btScalar* jacB = 0; - btScalar* jacA = 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->getNumDofs() + 6; - jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; - deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA; ++i) - { - btScalar j = jacA[i]; - btScalar l = deltaVelA[i]; - denom0 += j * l; - } - } - else if (rb0) - { - vec = (solverConstraint.m_angularComponentA).cross(rel_pos1); - if (angConstraint) - { - denom0 = constraintNormalAng.dot(solverConstraint.m_angularComponentA); - } - else - { - denom0 = rb0->getInvMass() + constraintNormalLin.dot(vec); - } - } - // - if (multiBodyB) - { - const int ndofB = multiBodyB->getNumDofs() + 6; - jacB = &data.m_jacobians[solverConstraint.m_jacBindex]; - deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex]; - for (int i = 0; i < ndofB; ++i) - { - btScalar j = jacB[i]; - btScalar l = deltaVelB[i]; - denom1 += j * l; - } - } - else if (rb1) - { - 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) - { - 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 penetration = isFriction ? 0 : posError; - - btScalar rel_vel = 0.f; - int ndofA = 0; - int ndofB = 0; - { - btVector3 vel1, vel2; - if (multiBodyA) - { - ndofA = multiBodyA->getNumDofs() + 6; - btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex]; - for (int i = 0; i < ndofA; ++i) - rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i]; - } - else if (rb0) - { - 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) - rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; - } - else if (rb1) - { - 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_appliedImpulse = 0.f; - solverConstraint.m_appliedPushImpulse = 0.f; - - { - btScalar positionalError = 0.f; - btScalar velocityError = (desiredVelocity - rel_vel) * damping; - - btScalar erp = infoGlobal.m_erp2; - - //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; - - btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; - - //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_rhsPenetration = 0.f; - } - /*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; - solverConstraint.m_upperLimit = upperLimit; - } - - return rel_vel; -} |