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

Improved URDF support for btMultiBody and separate graphics/collision/inertial frames and shapes

Browse Source 
Fix WinXP GetTickCount64 with a typedef
Expose debug drawing mode/flags in UI (hot keys A,D,L,W for now, buttons later)
GLInstancingRenderer: tweak near/far planes to allow closer approach of camera
btDiscreteDynamicsWorld: enable debug drawing for btGeneric6DofSpring2Constraint
btMultiBodyDynamicsWorld: enable basic debug drawing for btMultiBody
btMultibody: allow center-of-mass shift for prismatic and fixed constraint
This commit is contained in:
erwincoumans
2015-01-22 17:56:24 -08:00
parent 6e70f285e1
commit a159fbac69
28 changed files with 1022 additions and 674 deletions
Download Patch File Download Diff File Expand all files Collapse all files

52
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
View File

@@ -34,6 +34,7 @@ subject to the following restrictions:
#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
#include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
@@ -1306,6 +1307,57 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
}
}
break;
///note: the code for D6_SPRING_2_CONSTRAINT_TYPE is identical to D6_CONSTRAINT_TYPE, the D6_CONSTRAINT_TYPE+D6_SPRING_CONSTRAINT_TYPE will likely become obsolete/deprecated at some stage
case D6_SPRING_2_CONSTRAINT_TYPE:
{
{
btGeneric6DofSpring2Constraint* p6DOF = (btGeneric6DofSpring2Constraint*)constraint;
btTransform tr = p6DOF->getCalculatedTransformA();
if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
tr = p6DOF->getCalculatedTransformB();
if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
if (drawLimits)
{
tr = p6DOF->getCalculatedTransformA();
const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
btVector3 up = tr.getBasis().getColumn(2);
btVector3 axis = tr.getBasis().getColumn(0);
btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0));
axis = tr.getBasis().getColumn(1);
btScalar ay = p6DOF->getAngle(1);
btScalar az = p6DOF->getAngle(2);
btScalar cy = btCos(ay);
btScalar sy = btSin(ay);
btScalar cz = btCos(az);
btScalar sz = btSin(az);
btVector3 ref;
ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2];
ref[1] = -sz*axis[0] + cz*axis[1];
ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2];
tr = p6DOF->getCalculatedTransformB();
btVector3 normal = -tr.getBasis().getColumn(0);
btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;
btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit;
if (minFi > maxFi)
{
getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0, 0, 0), false);
}
else if (minFi < maxFi)
{
getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0, 0, 0), true);
}
tr = p6DOF->getCalculatedTransformA();
btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit;
btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit;
getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0, 0, 0));
}
}
break;
}
case SLIDER_CONSTRAINT_TYPE:
{
btSliderConstraint* pSlider = (btSliderConstraint*)constraint;

2
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
View File

@@ -151,7 +151,7 @@ public:
virtual void removeCollisionObject(btCollisionObject* collisionObject);
void debugDrawConstraint(btTypedConstraint* constraint);
virtual void debugDrawConstraint(btTypedConstraint* constraint);
virtual void debugDrawWorld();

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

@@ -137,16 +137,18 @@ void btMultiBody::setupFixed(int i,
const btVector3 &inertia,
int parent,
const btQuaternion &rotParentToThis,
const btVector3 &parentComToThisComOffset,
const btVector3 &parentComToThisPivotOffset,
const btVector3 &thisPivotToThisComOffset,
bool disableParentCollision)
{
btAssert(m_isMultiDof);
m_links[i].m_mass = mass;
m_links[i].m_inertia = inertia;
m_links[i].m_inertiaLocal = inertia;
m_links[i].m_parent = parent;
m_links[i].m_zeroRotParentToThis = rotParentToThis;
m_links[i].m_eVector = parentComToThisComOffset;
m_links[i].m_dVector = thisPivotToThisComOffset;
m_links[i].m_eVector = parentComToThisPivotOffset;
m_links[i].m_jointType = btMultibodyLink::eFixed;
m_links[i].m_dofCount = 0;
@@ -156,6 +158,7 @@ void btMultiBody::setupFixed(int i,
m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
//
m_links[i].updateCacheMultiDof();
//
//if(m_isMultiDof)
// resizeInternalMultiDofBuffers();
@@ -180,12 +183,12 @@ void btMultiBody::setupPrismatic(int i,
}
m_links[i].m_mass = mass;
m_links[i].m_inertia = inertia;
m_links[i].m_inertiaLocal = inertia;
m_links[i].m_parent = parent;
m_links[i].m_zeroRotParentToThis = rotParentToThis;
m_links[i].setAxisTop(0, 0., 0., 0.);
m_links[i].setAxisBottom(0, jointAxis);
m_links[i].m_eVector = parentComToThisComOffset;
m_links[i].m_eVector = parentComToThisComOffset;
m_links[i].m_cachedRotParentToThis = rotParentToThis;
m_links[i].m_jointType = btMultibodyLink::ePrismatic;
@@ -223,7 +226,7 @@ void btMultiBody::setupRevolute(int i,
}
m_links[i].m_mass = mass;
m_links[i].m_inertia = inertia;
m_links[i].m_inertiaLocal = inertia;
m_links[i].m_parent = parent;
m_links[i].m_zeroRotParentToThis = rotParentToThis;
m_links[i].setAxisTop(0, jointAxis);
@@ -265,7 +268,7 @@ void btMultiBody::setupSpherical(int i,
m_posVarCnt += 4;
m_links[i].m_mass = mass;
m_links[i].m_inertia = inertia;
m_links[i].m_inertiaLocal = inertia;
m_links[i].m_parent = parent;
m_links[i].m_zeroRotParentToThis = rotParentToThis;
m_links[i].m_dVector = thisPivotToThisComOffset;
@@ -307,7 +310,7 @@ void btMultiBody::setupPlanar(int i,
m_posVarCnt += 3;
m_links[i].m_mass = mass;
m_links[i].m_inertia = inertia;
m_links[i].m_inertiaLocal = inertia;
m_links[i].m_parent = parent;
m_links[i].m_zeroRotParentToThis = rotParentToThis;
m_links[i].m_dVector.setZero();
@@ -365,7 +368,7 @@ btScalar btMultiBody::getLinkMass(int i) const
const btVector3 & btMultiBody::getLinkInertia(int i) const
{
return m_links[i].m_inertia;
return m_links[i].m_inertiaLocal;
}
btScalar btMultiBody::getJointPos(int i) const
@@ -506,7 +509,7 @@ btScalar btMultiBody::getKineticEnergy() const
for (int i = 0; i < num_links; ++i) {
result += m_links[i].m_mass * vel[i+1].dot(vel[i+1]);
result += omega[i+1].dot(m_links[i].m_inertia * omega[i+1]);
result += omega[i+1].dot(m_links[i].m_inertiaLocal * omega[i+1]);
}
return 0.5f * result;
@@ -526,7 +529,7 @@ btVector3 btMultiBody::getAngularMomentum() const
for (int i = 0; i < num_links; ++i) {
rot_from_world[i+1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent+1];
result += (quatRotate(rot_from_world[i+1].inverse() , (m_links[i].m_inertia * omega[i+1])));
result += (quatRotate(rot_from_world[i+1].inverse() , (m_links[i].m_inertiaLocal * omega[i+1])));
}
return result;
@@ -1241,7 +1244,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
//
//adding damping terms (only)
btScalar linDampMult = 1., angDampMult = 1.;
zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertia * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().norm()),
zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().norm()),
linDampMult * m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().norm()));
// calculate Ihat_i^A
@@ -1252,9 +1255,9 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
0, m_links[i].m_mass, 0,
0, 0, m_links[i].m_mass),
//
btMatrix3x3(m_links[i].m_inertia[0], 0, 0,
0, m_links[i].m_inertia[1], 0,
0, 0, m_links[i].m_inertia[2])
btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0,
0, m_links[i].m_inertiaLocal[1], 0,
0, 0, m_links[i].m_inertiaLocal[2])
);
//
//p += vhat x Ihat vhat - done in a simpler way
@@ -1694,19 +1697,19 @@ void btMultiBody::stepVelocities(btScalar dt,
- (rot_from_world[i+1] * m_links[i].m_appliedTorque);
if (m_useGyroTerm)
{
zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertia * vel_top_angular[i+1] );
zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertiaLocal * vel_top_angular[i+1] );
}
zero_acc_bottom_linear[i+1] += m_links[i].m_inertia * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm());
zero_acc_bottom_linear[i+1] += m_links[i].m_inertiaLocal * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm());
// calculate Ihat_i^A
inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
inertia_top_right[i+1].setValue(m_links[i].m_mass, 0, 0,
0, m_links[i].m_mass, 0,
0, 0, m_links[i].m_mass);
inertia_bottom_left[i+1].setValue(m_links[i].m_inertia[0], 0, 0,
0, m_links[i].m_inertia[1], 0,
0, 0, m_links[i].m_inertia[2]);
inertia_bottom_left[i+1].setValue(m_links[i].m_inertiaLocal[0], 0, 0,
0, m_links[i].m_inertiaLocal[1], 0,
0, 0, m_links[i].m_inertiaLocal[2]);
}

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

@@ -61,7 +61,8 @@ public:
const btVector3 &inertia,
int parent,
const btQuaternion &rotParentToThis,
const btVector3 &parentComToThisComOffset,
const btVector3 &parentComToThisPivotOffset,
const btVector3 &thisPivotToThisComOffset,
bool disableParentCollision);

80
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
View File

@@ -20,7 +20,7 @@ subject to the following restrictions:
#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
#include "LinearMath/btQuickprof.h"
#include "btMultiBodyConstraint.h"
#include "LinearMath/btIDebugDraw.h"
@@ -755,3 +755,81 @@ void btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint*
{
m_multiBodyConstraints.remove(constraint);
}
void btMultiBodyDynamicsWorld::debugDrawWorld()
{
BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld");
bool drawConstraints = false;
if (getDebugDrawer())
{
int mode = getDebugDrawer()->getDebugMode();
if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
{
drawConstraints = true;
}
if (drawConstraints)
{
BT_PROFILE("btMultiBody stepPositions");
//integrate and update the Featherstone hierarchies
btAlignedObjectArray<btQuaternion> world_to_local;
btAlignedObjectArray<btVector3> local_origin;
for (int b = 0; b<m_multiBodies.size(); b++)
{
btMultiBody* bod = m_multiBodies[b];
int nLinks = bod->getNumLinks();
///base + num m_links
world_to_local.resize(nLinks + 1);
local_origin.resize(nLinks + 1);
world_to_local[0] = bod->getWorldToBaseRot();
local_origin[0] = bod->getBasePos();
{
btVector3 posr = local_origin[0];
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
btScalar quat[4] = { -world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w() };
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
getDebugDrawer()->drawTransform(tr, 0.1);
}
for (int k = 0; k<bod->getNumLinks(); k++)
{
const int parent = bod->getParent(k);
world_to_local[k + 1] = bod->getParentToLocalRot(k) * world_to_local[parent + 1];
local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), bod->getRVector(k)));
}
for (int m = 0; m<bod->getNumLinks(); m++)
{
int link = m;
int index = link + 1;
btVector3 posr = local_origin[index];
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
btScalar quat[4] = { -world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w() };
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
getDebugDrawer()->drawTransform(tr, 0.1);
}
}
}
}
btDiscreteDynamicsWorld::debugDrawWorld();
}

2
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
View File

@@ -54,5 +54,7 @@ public:
virtual void removeMultiBodyConstraint( btMultiBodyConstraint* constraint);
virtual void integrateTransforms(btScalar timeStep);
virtual void debugDrawWorld();
};
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H

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

@@ -355,7 +355,7 @@ struct btMultibodyLink
BT_DECLARE_ALIGNED_ALLOCATOR();
btScalar m_mass; // mass of link
btVector3 m_inertia; // inertia of link (local frame; diagonal)
btVector3 m_inertiaLocal; // inertia of link (local frame; diagonal)
int m_parent; // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
@@ -440,15 +440,15 @@ struct btMultibodyLink
btMultibodyLink()
: m_mass(1),
m_parent(-1),
m_zeroRotParentToThis(1, 0, 0, 0),
m_cachedRotParentToThis(1, 0, 0, 0),
m_zeroRotParentToThis(0, 0, 0, 1),
m_cachedRotParentToThis(0, 0, 0, 1),
m_collider(0),
m_flags(0),
m_dofCount(0),
m_posVarCount(0),
m_jointType(btMultibodyLink::eInvalid)
{
m_inertia.setValue(1, 1, 1);
m_inertiaLocal.setValue(1, 1, 1);
setAxisTop(0, 0., 0., 0.);
setAxisBottom(0, 1., 0., 0.);
m_dVector.setValue(0, 0, 0);
@@ -493,7 +493,7 @@ struct btMultibodyLink
case ePrismatic:
{
// m_cachedRotParentToThis never changes, so no need to update
m_cachedRVector = m_eVector + pJointPos[0] * getAxisBottom(0);
m_cachedRVector = quatRotate(m_cachedRotParentToThis,m_eVector) + pJointPos[0] * getAxisBottom(0);
break;
}
@@ -516,7 +516,7 @@ struct btMultibodyLink
case eFixed:
{
m_cachedRotParentToThis = m_zeroRotParentToThis;
m_cachedRVector = quatRotate(m_cachedRotParentToThis,m_eVector);
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
break;
}

5
src/LinearMath/btQuickprof.cpp
View File

@@ -43,6 +43,11 @@ static btClock gProfileClock;
#include <Xtl.h>
#else //_XBOX
#include <windows.h>
#if WINVER < 0x0600
ULONGLONG GetTickCount64() { return GetTickCount(); }
#endif
#endif //_XBOX
#include <time.h>