diff --git a/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp b/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp index b5164da7b..b356edf88 100644 --- a/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp +++ b/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp @@ -38,6 +38,7 @@ subject to the following restrictions: #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyLink.h" +#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h" @@ -209,15 +210,15 @@ void FeatherstoneMultiBodyDemo::initPhysics() } - createFeatherstoneMultiBody(world, 10, btVector3 (20,29.5,-2), true); + createFeatherstoneMultiBody(world, 3, btVector3 (20,29.5,-2), true, false);//true); - createFeatherstoneMultiBody(world, 5, btVector3 (0,29.5,-2), false); + createFeatherstoneMultiBody(world, 5, btVector3 (0,29.5,-2), false,false); } -void FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition,bool isFixedBase) +void FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition,bool isFixedBase, bool usePrismatic) { { int n_links = numLinks; @@ -259,10 +260,11 @@ void FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMultiBodyDyn const int child_link_num = link_num_counter++; - if (0)//i==(n_links-1)) + if (usePrismatic && i==(n_links-1)) { bod->setupPrismatic(child_link_num, mass, inertia, this_link_num, parent_to_child, joint_axis_child_prismatic, quatRotate(parent_to_child , pos)); + } else { bod->setupRevolute(child_link_num, mass, inertia, this_link_num,parent_to_child, joint_axis_child_hinge, @@ -271,6 +273,13 @@ void FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMultiBodyDyn bod->setJointPos(child_link_num, initial_joint_angle); this_link_num = i; } + + //add some constraint limit + if (usePrismatic) + { + btMultiBodyConstraint* limit = new btMultiBodyJointLimitConstraint(bod,n_links-1,2,2); + world->addMultiBodyConstraint(limit); + } } //add a collider for the base diff --git a/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.h b/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.h index c38343c38..039afb574 100644 --- a/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.h +++ b/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.h @@ -53,7 +53,7 @@ class FeatherstoneMultiBodyDemo : public PlatformDemoApplication btDefaultCollisionConfiguration* m_collisionConfiguration; - void createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition,bool isFixedBase); + void createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition,bool isFixedBase, bool usePrismatic); public: diff --git a/src/BulletDynamics/CMakeLists.txt b/src/BulletDynamics/CMakeLists.txt index c329f7940..f46956079 100644 --- a/src/BulletDynamics/CMakeLists.txt +++ b/src/BulletDynamics/CMakeLists.txt @@ -26,6 +26,7 @@ SET(BulletDynamics_SRCS Featherstone/btMultiBody.cpp Featherstone/btMultiBodyConstraintSolver.cpp Featherstone/btMultiBodyDynamicsWorld.cpp + Featherstone/btMultiBodyJointLimitConstraint.cpp ) SET(Root_HDRS @@ -72,6 +73,8 @@ SET(Featherstone_HDRS Featherstone/btMultiBodyLink.h Featherstone/btMultiBodyLinkCollider.h Featherstone/btMultiBodySolverConstraint.h + Featherstone/btMultiBodyConstraint.h + Featherstone/btMultiBodyJointLimitConstraint.h ) SET(Character_HDRS Character/btCharacterControllerInterface.h diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index ab2ab9338..d2a38ce38 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -16,19 +16,91 @@ subject to the following restrictions: #ifndef BT_MULTIBODY_CONSTRAINT_H #define BT_MULTIBODY_CONSTRAINT_H +#include "LinearMath/btScalar.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "btMultiBody.h" + +class btMultiBody; + class btMultiBodyConstraint { - +protected: + + btMultiBody* m_bodyA; + btMultiBody* m_bodyB; + int m_linkA; + int m_linkB; + + int m_num_rows; + int m_jac_size_A; + int m_jac_size_both; + int m_pos_offset; + + // data block laid out as follows: + // cached impulses. (one per row.) + // jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc) + // positions. (one per row.) + btAlignedObjectArray m_data; + public: - int getIslandIdA() const + + btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows) + :m_bodyA(bodyA), + m_bodyB(bodyB), + m_linkA(linkA), + m_linkB(linkB), + m_num_rows(numRows) + { + 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 { return 0; } - int getIslandIdB() const + virtual int getIslandIdB() const { return 0; } + virtual void update()=0; + + // current constraint position + // constraint is pos >= 0 for unilateral, or pos = 0 for bilateral + // NOTE: position ignored for friction rows. + btScalar getPosition(int row) const + { + return m_data[m_pos_offset + row]; + } + + void setPosition(int row, btScalar pos) + { + m_data[m_pos_offset + row] = pos; + } + + // 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. + btScalar* jacobianA(int row) + { + return &m_data[m_num_rows + row * m_jac_size_both]; + } + const btScalar* jacobianA(int row) const + { + return &m_data[m_num_rows + (row * m_jac_size_both)]; + } + btScalar* jacobianB(int row) + { + return &m_data[m_num_rows + (row * m_jac_size_both) + m_jac_size_A]; + } + const btScalar* jacobianB(int row) const + { + return &m_data[m_num_rows + (row * m_jac_size_both) + m_jac_size_A]; + } + + }; #endif //BT_MULTIBODY_CONSTRAINT_H diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 11d12904b..d9c53fc07 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -18,8 +18,9 @@ subject to the following restrictions: #include "btMultiBodyLinkCollider.h" #include "BulletDynamics/ConstraintSolver/btSolverBody.h" +#include "btMultiBodyConstraint.h" - +#include "LinearMath/btQuickprof.h" btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) { @@ -173,6 +174,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) { + BT_PROFILE("setupMultiBodyContactConstraint"); btVector3 rel_pos1; btVector3 rel_pos2; @@ -484,6 +486,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol 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"); btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing(); solverConstraint.m_frictionIndex = frictionIndex; bool isFriction = true; @@ -576,7 +579,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* /////setup the friction constraints #define ENABLE_FRICTION #ifdef ENABLE_FRICTION - solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); + solverConstraint.m_frictionIndex = frictionIndex; #if ROLLING_FRICTION btVector3 angVelA(0,0,0),angVelB(0,0,0); if (rb0) @@ -711,6 +714,16 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol convertMultiBodyContact(manifold,infoGlobal); } } + + //also convert the multibody constraints, if any + + for (int i=0;iupdate(); + } + + } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index 29be381bc..f359af7fd 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -393,6 +393,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2()); btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; + m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size()); for (i=0;igetLinkCollider(0)->getIslandTag(); +} + +int btMultiBodyJointLimitConstraint::getIslandIdB() const +{ + return m_bodyB->getLinkCollider(0)->getIslandTag(); +} + +void btMultiBodyJointLimitConstraint::update() +{ + // only positions need to be updated -- jacobians and force + // directions were set in the ctor and never change. + + // row 0: the lower bound + setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); + + // row 1: the upper bound + setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA)); +} \ No newline at end of file diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h new file mode 100644 index 000000000..acdd4db47 --- /dev/null +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h @@ -0,0 +1,40 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H +#define BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H + +#include "btMultiBodyConstraint.h" + +class btMultiBodyJointLimitConstraint : public btMultiBodyConstraint +{ +protected: + + btScalar m_lowerBound; + btScalar m_upperBound; +public: + + btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper); + virtual ~btMultiBodyJointLimitConstraint(); + + virtual int getIslandIdA() const; + virtual int getIslandIdB() const; + + virtual void update(); + +}; + +#endif //BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H +