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

preparing for stabilization investigation: useRK4 is now a btMultiBody flag (not world's), reenabled global velocities (as a flag-controlled option), made the test application easier to handle for multiple multibodiez and added a max coordinate multibody (created from btMultiBody)

Browse Source
This commit is contained in:
kubas
2014-01-09 01:14:48 +01:00
parent 2cbcd86de9
commit aa87e47d2d
4 changed files with 39 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -104,7 +104,8 @@ btMultiBody::btMultiBody(int n_links,
m_dofCount(0),
__posUpdated(false),
m_isMultiDof(multiDof),
m_posVarCnt(0)
m_posVarCnt(0),
m_useRK4(false), m_useGlobalVelocities(false)
{
if(!m_isMultiDof)
@@ -1155,7 +1156,6 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
rot_from_world[0] = rot_from_parent[0];
//
bool useGlobalVelocities = false;
for (int i = 0; i < num_links; ++i) {
const int parent = m_links[i].m_parent;
rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
@@ -1201,7 +1201,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
// now set vhat_i to its true value by doing
// vhat_i += qidot * shat_i
if(!useGlobalVelocities)
if(!m_useGlobalVelocities)
{
spatJointVel.setZero();
@@ -1494,37 +1494,37 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
/////
/////////////////////
//if(useGlobalVelocities)
//{
// for (int i = 0; i < num_links; ++i)
// {
// const int parent = m_links[i].m_parent;
// //rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); /// <- done
// //rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; /// <- done
//
// fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
// fromWorld.m_rotMat = rot_from_world[i+1];
//
// // vhat_i = i_xhat_p(i) * vhat_p(i)
// fromParent.transform(spatVel[parent+1], spatVel[i+1]);
// //nice alternative below (using operator *) but it generates temps
// /////////////////////////////////////////////////////////////
if(m_useGlobalVelocities)
{
for (int i = 0; i < num_links; ++i)
{
const int parent = m_links[i].m_parent;
//rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); /// <- done
//rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; /// <- done
fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
fromWorld.m_rotMat = rot_from_world[i+1];
// vhat_i = i_xhat_p(i) * vhat_p(i)
fromParent.transform(spatVel[parent+1], spatVel[i+1]);
//nice alternative below (using operator *) but it generates temps
/////////////////////////////////////////////////////////////
// // now set vhat_i to its true value by doing
// // vhat_i += qidot * shat_i
// spatJointVel.setZero();
// now set vhat_i to its true value by doing
// vhat_i += qidot * shat_i
spatJointVel.setZero();
// for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
// spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
//
// // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint
// spatVel[i+1] += spatJointVel;
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
// remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint
spatVel[i+1] += spatJointVel;
// fromWorld.transformInverseRotationOnly(spatVel[i+1], m_links[i].m_absFrameTotVelocity);
// fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity);
// }
//}
fromWorld.transformInverseRotationOnly(spatVel[i+1], m_links[i].m_absFrameTotVelocity);
fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity);
}
}
}
#endif

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

@@ -489,6 +489,11 @@ public:
bool isMultiDof() { return m_isMultiDof; }
void finalizeMultiDof();
void useRK4Integration(bool use) { m_useRK4 = use; }
bool isUsingRK4Integration() const { return m_useRK4; }
void useGlobalVelocities(bool use) { m_useGlobalVelocities = use; }
bool isUsingGlobalVelocities() const { return m_useGlobalVelocities; }
bool __posUpdated;
private:
@@ -530,6 +535,8 @@ private:
btAlignedObjectArray<btMultibodyLink> m_links; // array of m_links, excluding the base. index from 0 to num_links-1.
btAlignedObjectArray<btMultiBodyLinkCollider*> m_colliders;
int m_dofCount, m_posVarCnt;
bool m_useRK4, m_useGlobalVelocities;
//
// realBuf:

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

@@ -381,7 +381,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
:btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration),
m_multiBodyConstraintSolver(constraintSolver), m_useRK4ForMultiBodies(false)
m_multiBodyConstraintSolver(constraintSolver)
{
//split impulse is not yet supported for Featherstone hierarchies
getSolverInfo().m_splitImpulse = false;
@@ -467,7 +467,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
if(bod->isMultiDof())
{
if(!m_useRK4ForMultiBodies)
if(!bod->isUsingRK4Integration())
bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
else
{

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

@@ -34,7 +34,6 @@ protected:
btAlignedObjectArray<btMultiBodyConstraint*> m_sortedMultiBodyConstraints;
btMultiBodyConstraintSolver* m_multiBodyConstraintSolver;
MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback;
bool m_useRK4ForMultiBodies;
virtual void calculateSimulationIslands();
virtual void updateActivationState(btScalar timeStep);
@@ -53,8 +52,5 @@ public:
virtual void addMultiBodyConstraint( btMultiBodyConstraint* constraint);
virtual void removeMultiBodyConstraint( btMultiBodyConstraint* constraint);
void useRK4IntegrationForMultiBodies(bool use) { m_useRK4ForMultiBodies = use; }
bool isUsingRK4IntegrationForMultiBodies() const { return m_useRK4ForMultiBodies; }
};
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H