diff --git a/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp b/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp index 6911893b5..3affd399a 100644 --- a/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp +++ b/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp @@ -264,7 +264,8 @@ btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMult btMultiBody * bod = new btMultiBody(n_links, mass, inertia, isFixedBase, canSleep); - +// bod->setHasSelfCollision(false); + //btQuaternion orn(btVector3(0,0,1),-0.25*SIMD_HALF_PI);//0,0,0,1); btQuaternion orn(0,0,0,1); bod->setBasePos(basePosition); @@ -297,15 +298,17 @@ btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMult const int child_link_num = link_num_counter++; + bool disableParentCollision = false; + 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)); + parent_to_child, joint_axis_child_prismatic, quatRotate(parent_to_child , pos),disableParentCollision); } else { bod->setupRevolute(child_link_num, mass, inertia, this_link_num,parent_to_child, joint_axis_child_hinge, - joint_axis_position,quatRotate(parent_to_child , (pos - joint_axis_position))); + joint_axis_position,quatRotate(parent_to_child , (pos - joint_axis_position)),disableParentCollision); } bod->setJointPos(child_link_num, initial_joint_angle); this_link_num = i; @@ -381,7 +384,7 @@ btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMult body->setWorldTransform(tr); col->setWorldTransform(tr); - world->addCollisionObject(col, 2,1); + world->addCollisionObject(col, 2,1+2); col->setFriction(friction); bod->setBaseCollider(col); @@ -415,7 +418,7 @@ btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMult tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); col->setWorldTransform(tr); col->setFriction(friction); - world->addCollisionObject(col,2,1); + world->addCollisionObject(col,2,1+2); bod->getLink(i).m_collider=col; //app->drawBox(halfExtents, pos,quat); diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 4c9107be1..b04bc4f95 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -93,7 +93,8 @@ btMultiBody::btMultiBody(int n_links, m_linearDamping(0.04f), m_angularDamping(0.04f), m_useGyroTerm(true), - m_maxAppliedImpulse(1000.f) + m_maxAppliedImpulse(1000.f), + m_hasSelfCollision(true) { links.resize(n_links); @@ -115,7 +116,8 @@ void btMultiBody::setupPrismatic(int i, int parent, const btQuaternion &rot_parent_to_this, const btVector3 &joint_axis, - const btVector3 &r_vector_when_q_zero) + const btVector3 &r_vector_when_q_zero, + bool disableParentCollision) { links[i].mass = mass; links[i].inertia = inertia; @@ -126,6 +128,9 @@ void btMultiBody::setupPrismatic(int i, links[i].e_vector = r_vector_when_q_zero; links[i].is_revolute = false; links[i].cached_rot_parent_to_this = rot_parent_to_this; + if (disableParentCollision) + links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; + links[i].updateCache(); } @@ -136,7 +141,8 @@ void btMultiBody::setupRevolute(int i, const btQuaternion &zero_rot_parent_to_this, const btVector3 &joint_axis, const btVector3 &parent_axis_position, - const btVector3 &my_axis_position) + const btVector3 &my_axis_position, + bool disableParentCollision) { links[i].mass = mass; links[i].inertia = inertia; @@ -147,6 +153,8 @@ void btMultiBody::setupRevolute(int i, links[i].d_vector = my_axis_position; links[i].e_vector = parent_axis_position; links[i].is_revolute = true; + if (disableParentCollision) + links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION; links[i].updateCache(); } diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index 6fad660fd..9f2e99abd 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -59,7 +59,9 @@ public: int parent, const btQuaternion &rot_parent_to_this, // rotate points in parent frame to my frame. const btVector3 &joint_axis, // in my frame - const btVector3 &r_vector_when_q_zero); // vector from parent COM to my COM, in my frame, when q = 0. + const btVector3 &r_vector_when_q_zero, // vector from parent COM to my COM, in my frame, when q = 0. + bool disableParentCollision=false + ); void setupRevolute(int i, // 0 to num_links-1 btScalar mass, @@ -68,7 +70,8 @@ public: const btQuaternion &zero_rot_parent_to_this, // rotate points in parent frame to this frame, when q = 0 const btVector3 &joint_axis, // in my frame const btVector3 &parent_axis_position, // vector from parent COM to joint axis, in PARENT frame - const btVector3 &my_axis_position); // vector from joint axis to my COM, in MY frame + const btVector3 &my_axis_position, // vector from joint axis to my COM, in MY frame + bool disableParentCollision=false); const btMultibodyLink& getLink(int index) const { @@ -384,6 +387,15 @@ public: m_maxAppliedImpulse = maxImp; } + void setHasSelfCollision(bool hasSelfCollision) + { + m_hasSelfCollision = hasSelfCollision; + } + bool hasSelfCollision() const + { + return m_hasSelfCollision; + } + private: btMultiBody(const btMultiBody &); // not implemented void operator=(const btMultiBody &); // not implemented @@ -448,6 +460,7 @@ private: btScalar m_angularDamping; bool m_useGyroTerm; btScalar m_maxAppliedImpulse; + bool m_hasSelfCollision; }; #endif diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h index 12f5b80ab..eaaf87d72 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -20,7 +20,10 @@ subject to the following restrictions: #include "LinearMath/btVector3.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" - +enum btMultiBodyLinkFlags +{ + BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1 +}; // // Link struct // @@ -64,6 +67,7 @@ struct btMultibodyLink btScalar joint_torque; class btMultiBodyLinkCollider* m_collider; + int m_flags; // ctor: set some sensible defaults btMultibodyLink() @@ -74,7 +78,8 @@ struct btMultibodyLink is_revolute(false), cached_rot_parent_to_this(1, 0, 0, 0), joint_torque(0), - m_collider(0) + m_collider(0), + m_flags(0) { inertia.setValue(1, 1, 1); axis_top.setValue(0, 0, 0); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h b/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h index 62f0c34cd..3cc27a07a 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h @@ -33,6 +33,7 @@ public: :m_multiBody(multiBody), m_link(link) { + m_checkCollideWith = true; //we need to remove the 'CF_STATIC_OBJECT' flag, otherwise links/base doesn't merge islands //this means that some constraints might point to bodies that are not in the islands, causing crashes //if (link>=0 || (multiBody && !multiBody->hasFixedBase())) @@ -59,6 +60,32 @@ public: return 0; } + virtual bool checkCollideWithOverride(const btCollisionObject* co) const + { + const btMultiBodyLinkCollider* other = btMultiBodyLinkCollider::upcast(co); + if (!other) + return true; + if (other->m_multiBody != this->m_multiBody) + return true; + if (!m_multiBody->hasSelfCollision()) + return false; + + //check if 'link' has collision disabled + if (m_link>=0) + { + const btMultibodyLink& link = m_multiBody->getLink(this->m_link); + if ((link.m_flags&BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && link.parent == other->m_link) + return false; + } + + if (other->m_link>=0) + { + const btMultibodyLink& otherLink = other->m_multiBody->getLink(other->m_link); + if ((otherLink.m_flags& BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION) && otherLink.parent == this->m_link) + return false; + } + return true; + } }; #endif //BT_FEATHERSTONE_LINK_COLLIDER_H