Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

add option to disable btMultiBody self-collision (for the entire hierarchy) or to disable parent-child collision for jointed links.

Browse Source
This commit is contained in:
erwin.coumans@gmail.com
2013-10-08 00:50:40 +00:00
parent 34d975143e
commit d4640227ce
5 changed files with 68 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp
View File

@@ -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);

14
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -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();
}

17
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@@ -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

9
src/BulletDynamics/Featherstone/btMultiBodyLink.h
View File

@@ -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);

27
src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h
View File

@@ -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