diff --git a/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp b/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp index b356edf88..20253f947 100644 --- a/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp +++ b/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp @@ -202,7 +202,6 @@ void FeatherstoneMultiBodyDemo::initPhysics() btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia); btRigidBody* body = new btRigidBody(rbInfo); - m_dynamicsWorld->addRigidBody(body);//,1,1+2); } } @@ -210,7 +209,7 @@ void FeatherstoneMultiBodyDemo::initPhysics() } - createFeatherstoneMultiBody(world, 3, btVector3 (20,29.5,-2), true, false);//true); + createFeatherstoneMultiBody(world, 5, btVector3 (20,29.5,-2), true, true); createFeatherstoneMultiBody(world, 5, btVector3 (0,29.5,-2), false,false); @@ -277,7 +276,7 @@ void FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMultiBodyDyn //add some constraint limit if (usePrismatic) { - btMultiBodyConstraint* limit = new btMultiBodyJointLimitConstraint(bod,n_links-1,2,2); + btMultiBodyConstraint* limit = new btMultiBodyJointLimitConstraint(bod,n_links-1,2,3); world->addMultiBodyConstraint(limit); } } diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index 98849fc10..49443fbff 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -31,7 +31,7 @@ #include "LinearMath/btAlignedObjectArray.h" -#include "btMultibodyLink.h" +#include "btMultiBodyLink.h" class btMultiBodyLinkCollider; class btMultiBody @@ -287,6 +287,7 @@ public: } void setCompanionId(int id) { + //printf("for %p setCompanionId(%d)\n",this, id); m_companionId = id; } private: diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index d2a38ce38..e6bd2b134 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -36,6 +36,8 @@ protected: int m_jac_size_both; int m_pos_offset; + bool m_isUnilateral; + // data block laid out as follows: // cached impulses. (one per row.) // jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc) @@ -44,32 +46,41 @@ protected: public: - btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows) + 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_num_rows(numRows), + m_isUnilateral(isUnilateral) { 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); } - virtual int getIslandIdA() const + virtual int getIslandIdA() const =0; + virtual int getIslandIdB() const =0; + + virtual void update()=0; + + int getNumRows() const { - return 0; - } - virtual int getIslandIdB() const - { - return 0; + return m_num_rows; } - virtual void update()=0; + btMultiBody* getMultiBodyA() + { + return m_bodyA; + } + btMultiBody* getMultiBodyB() + { + return m_bodyB; + } // current constraint position // constraint is pos >= 0 for unilateral, or pos = 0 for bilateral - // NOTE: position ignored for friction rows. + // NOTE: ignored position for friction rows. btScalar getPosition(int row) const { return m_data[m_pos_offset + row]; @@ -80,6 +91,12 @@ public: m_data[m_pos_offset + row] = pos; } + + bool isUnilateral() const + { + return m_isUnilateral; + } + // jacobian blocks. // each of size 6 + num_links. (jacobian2 is null if no body2.) // format: 3 'omega' coefficients, 3 'v' coefficients, then the 'qdot' coefficients. @@ -100,6 +117,7 @@ public: return &m_data[m_num_rows + (row * m_jac_size_both) + m_jac_size_A]; } + }; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index d9c53fc07..041779ea9 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -28,11 +28,12 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl //solve featherstone non-contact constraints + //printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size()); for (int j=0;jgetNumLinks() + 6; + for (int i = 0; i < ndofA; ++i) + deltaVelADotn += m_jacobians[c.m_jacAindex+i] * m_deltaVelocities[c.m_deltaVelAindex+i]; + } + + if (c.m_multiBodyB) + { + ndofB = c.m_multiBodyB->getNumLinks() + 6; + for (int i = 0; i < ndofB; ++i) + deltaVelBDotn += m_jacobians[c.m_jacBindex+i] * m_deltaVelocities[c.m_deltaVelBindex+i]; + } + + + deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom + deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv; + const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; + + if (sum < c.m_lowerLimit) + { + deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; + c.m_appliedImpulse = c.m_lowerLimit; + } + else if (sum > c.m_upperLimit) + { + deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; + c.m_appliedImpulse = c.m_upperLimit; + } + else + { + c.m_appliedImpulse = sum; + } + + if (c.m_multiBodyA) + { + applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA); + c.m_multiBodyA->applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse); + } + if (c.m_multiBodyB) + { + applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB); + c.m_multiBodyB->applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse); + } +} + void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, const btVector3& contactNormal, @@ -190,8 +246,6 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody; btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody; -// btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); -// btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); if (bodyA) rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); if (bodyB) @@ -201,7 +255,6 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol if (multiBodyA) { - //solverConstraint.m_jacA = const int ndofA = multiBodyA->getNumLinks() + 6; solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId(); @@ -436,18 +489,6 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol erp = infoGlobal.m_erp; } - // const btScalar ALLOWED_PENETRATION = btScalar(0.01); - -// float baumgarte_coeff = 0.3; - /// float one_over_dt = 1.f/infoGlobal.m_timeStep; - // btScalar minus_vnew = -penetration * baumgarte_coeff * one_over_dt; - // float myrhs = minus_vnew*solverConstraint.m_jacDiagABInv;//?? - - // solverConstraint.m_rhs = minus_vnew*solverConstraint.m_jacDiagABInv;//?? - //solverConstraint.m_rhsPenetration = 0.f; - - //penetration=0.f; -#if 1 if (penetration>0) { positionalError = 0; @@ -464,9 +505,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) { //combine position and velocity into rhs - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;//-solverConstraint.m_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv; - //solverConstraint.m_rhs = velocityImpulse;//-solverConstraint.m_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv; - + solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; solverConstraint.m_rhsPenetration = 0.f; } else @@ -475,7 +514,6 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol solverConstraint.m_rhs = velocityImpulse; solverConstraint.m_rhsPenetration = penetrationImpulse; } -#endif solverConstraint.m_cfm = 0.f; solverConstraint.m_lowerLimit = 0; @@ -484,6 +522,260 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol } + + +void btMultiBodyConstraintSolver::setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow, + btScalar* jacOrgA,btScalar* jacOrgB, + btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff, + const btContactSolverInfo& infoGlobal) +{ + + BT_PROFILE("setupMultiBodyContactConstraint"); + + btMultiBody* multiBodyA = constraintRow.m_multiBodyA; + btMultiBody* multiBodyB = constraintRow.m_multiBodyB; + + if (multiBodyA) + { + + const int ndofA = multiBodyA->getNumLinks() + 6; + + constraintRow.m_deltaVelAindex = multiBodyA->getCompanionId(); + + if (constraintRow.m_deltaVelAindex <0) + { + constraintRow.m_deltaVelAindex = m_deltaVelocities.size(); + multiBodyA->setCompanionId(constraintRow.m_deltaVelAindex); + m_deltaVelocities.resize(m_deltaVelocities.size()+ndofA); + } else + { + btAssert(m_deltaVelocities.size() >= constraintRow.m_deltaVelAindex+ndofA); + } + + constraintRow.m_jacAindex = m_jacobians.size(); + m_jacobians.resize(m_jacobians.size()+ndofA); + m_deltaVelocitiesUnitImpulse.resize(m_deltaVelocitiesUnitImpulse.size()+ndofA); + btAssert(m_jacobians.size() == m_deltaVelocitiesUnitImpulse.size()); + for (int i=0;icalcAccelerationDeltas(&m_jacobians[constraintRow.m_jacAindex],delta,scratch_r, scratch_v); + } + + if (multiBodyB) + { + const int ndofB = multiBodyB->getNumLinks() + 6; + + constraintRow.m_deltaVelBindex = multiBodyB->getCompanionId(); + if (constraintRow.m_deltaVelBindex <0) + { + constraintRow.m_deltaVelBindex = m_deltaVelocities.size(); + multiBodyB->setCompanionId(constraintRow.m_deltaVelBindex); + m_deltaVelocities.resize(m_deltaVelocities.size()+ndofB); + } + + constraintRow.m_jacBindex = m_jacobians.size(); + m_jacobians.resize(m_jacobians.size()+ndofB); + + for (int i=0;icalcAccelerationDeltas(&m_jacobians[constraintRow.m_jacBindex],&m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],scratch_r, 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 = &m_jacobians[constraintRow.m_jacAindex]; + lambdaA = &m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex]; + for (int i = 0; i < ndofA; ++i) + { + float j = jacA[i] ; + float l =lambdaA[i]; + denom0 += j*l; + } + } + if (multiBodyB) + { + const int ndofB = multiBodyB->getNumLinks() + 6; + jacB = &m_jacobians[constraintRow.m_jacBindex]; + lambdaB = &m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex]; + for (int i = 0; i < ndofB; ++i) + { + float j = jacB[i] ; + float 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]; + } + } + + float d = denom0+denom1; + if (btFabs(d)>SIMD_EPSILON) + { + + constraintRow.m_jacDiagABInv = 1.f/(d); + } else + { + constraintRow.m_jacDiagABInv = 1.f; + } + + } + + + //compute rhs and remaining constraintRow fields + + + + btScalar restitution = 0.f; + + + btScalar rel_vel = 0.f; + int ndofA = 0; + int ndofB = 0; + { + + btVector3 vel1,vel2; + if (multiBodyA) + { + ndofA = multiBodyA->getNumLinks() + 6; + btScalar* jacA = &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 = &m_jacobians[constraintRow.m_jacBindex]; + for (int i = 0; i < ndofB ; ++i) + rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i]; + + } + + constraintRow.m_friction = combinedFrictionCoeff; + + + restitution = restitutionCurve(rel_vel, combinedRestitutionCoeff); + if (restitution <= btScalar(0.)) + { + restitution = 0.f; + }; + } + + /* + ///warm starting (or zero if disabled) + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + constraintRow.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; + + if (constraintRow.m_appliedImpulse) + { + if (multiBodyA) + { + btScalar impulse = constraintRow.m_appliedImpulse; + btScalar* deltaV = &m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex]; + multiBodyA->applyDeltaVee(deltaV,impulse); + applyDeltaVee(deltaV,impulse,constraintRow.m_deltaVelAindex,ndofA); + } + if (multiBodyB) + { + btScalar impulse = constraintRow.m_appliedImpulse; + btScalar* deltaV = &m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex]; + multiBodyB->applyDeltaVee(deltaV,impulse); + applyDeltaVee(deltaV,impulse,constraintRow.m_deltaVelBindex,ndofB); + } + } + } + else + */ + { + constraintRow.m_appliedImpulse = 0.f; + } + + constraintRow.m_appliedPushImpulse = 0.f; + + { + + + btScalar positionalError = 0.f; + btScalar velocityError = restitution - rel_vel;// * damping; + + + btScalar erp = infoGlobal.m_erp2; + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + erp = infoGlobal.m_erp; + } + + // const btScalar ALLOWED_PENETRATION = btScalar(0.01); + +// float baumgarte_coeff = 0.3; + /// float one_over_dt = 1.f/infoGlobal.m_timeStep; + // btScalar minus_vnew = -penetration * baumgarte_coeff * one_over_dt; + // float myrhs = minus_vnew*solverConstraint.m_jacDiagABInv;//?? + + // solverConstraint.m_rhs = minus_vnew*solverConstraint.m_jacDiagABInv;//?? + //solverConstraint.m_rhsPenetration = 0.f; + + //penetration=0.f; + + if (penetration>0) + { + positionalError = 0; + velocityError = -penetration / infoGlobal.m_timeStep; + + } else + { + positionalError = -penetration * erp/infoGlobal.m_timeStep; + } + + btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv; + btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv; + + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + //combine position and velocity into rhs + constraintRow.m_rhs = penetrationImpulse+velocityImpulse; + constraintRow.m_rhsPenetration = 0.f; + + } else + { + //split position and velocity into rhs and m_rhsPenetration + constraintRow.m_rhs = velocityImpulse; + constraintRow.m_rhsPenetration = penetrationImpulse; + } + + + + + constraintRow.m_cfm = 0.f; + constraintRow.m_lowerLimit = 0; + constraintRow.m_upperLimit = 1e10f; + } + +} + btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) { BT_PROFILE("addMultiBodyFrictionConstraint"); @@ -717,12 +1009,24 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol //also convert the multibody constraints, if any + for (int i=0;iupdate(); + + for (int row=0;rowgetNumRows();row++) + { + + + btMultiBodySolverConstraint& constraintRow = m_multiBodyNonContactConstraints.expandNonInitializing(); + constraintRow.m_multiBodyA = c->getMultiBodyA(); + constraintRow.m_multiBodyB = c->getMultiBodyB(); + + btScalar penetration = c->getPosition(row);//rhs = c->computeRhs(row,infoGlobal.m_timeStep); + setupMultiBodyJointLimitConstraint(constraintRow,c->jacobianA(row),c->jacobianB(row),penetration,0,0,infoGlobal); + } } - } @@ -736,6 +1040,7 @@ btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int void btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher) { + //printf("solveMultiBodyGroup start\n"); m_tmpMultiBodyConstraints = multiBodyConstraints; m_tmpNumMultiBodyConstraints = numMultiBodyConstraints; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h index 59d5be129..8ab334782 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h @@ -48,10 +48,17 @@ protected: int m_tmpNumMultiBodyConstraints; void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c); + void resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c); + void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal); btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0); + void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow, + btScalar* jacA,btScalar* jacB, + btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff, + const btContactSolverInfo& infoGlobal); + void setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, const btVector3& contactNormal, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index f359af7fd..ae1f4ace9 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -273,14 +273,6 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager:: int i; //find the first constraint for this island - for (i=0;i