AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-13 21:30:09 +00:00
Code Issues Releases Wiki Activity

made the multiDof-singleDof disctinction a bit cleaner

Browse Source
This commit is contained in:
kubas 2014-01-09 00:58:31 +01:00
parent e5372f3712
commit 4eac9a11f3
2 changed files with 51 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -85,7 +85,8 @@ btMultiBody::btMultiBody(int n_links,
btScalar mass,
const btVector3 &inertia,
bool fixedBase,
bool canSleep)
bool canSleep,
bool multiDof)
: m_baseQuat(0, 0, 0, 1),
m_baseMass(mass),
m_baseInertia(inertia),
@ -101,54 +102,24 @@ btMultiBody::btMultiBody(int n_links,
m_maxAppliedImpulse(1000.f),
m_hasSelfCollision(true),
m_dofCount(n_links),
__posUpdated(false),
m_posVarCnt(-1) //-1 => not calculated yet/invalid
__posUpdated(false),
m_isMultiDof(multiDof),
m_posVarCnt(0)
{
if(!m_isMultiDof)
{
m_vectorBuf.resize(2*n_links);
m_realBuf.resize(6 + 2*n_links);
m_posVarCnt = n_links;
}
m_links.resize(n_links);
m_vectorBuf.resize(2*n_links);
m_matrixBuf.resize(n_links + 1);
m_realBuf.resize(6 + 2*n_links);
m_basePos.setValue(0, 0, 0);
m_baseForce.setValue(0, 0, 0);
m_baseTorque.setValue(0, 0, 0);
m_isMultiDof = false;
}
btMultiBody::btMultiBody(int n_links, int n_dofs, btScalar mass,
const btVector3 &inertia,
bool fixedBase,
bool canSleep)
: m_baseQuat(0, 0, 0, 1),
m_baseMass(mass),
m_baseInertia(inertia),
m_fixedBase(fixedBase),
m_awake(true),
m_canSleep(canSleep),
m_sleepTimer(0),
m_baseCollider(0),
m_linearDamping(0.04f),
m_angularDamping(0.04f),
m_useGyroTerm(true),
m_maxAppliedImpulse(1000.f),
m_hasSelfCollision(true),
m_dofCount(n_dofs),
__posUpdated(false),
m_posVarCnt(-1) //-1 => not calculated yet/invalid
{
m_links.resize(n_links);
m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices
m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices)
m_matrixBuf.resize(n_links + 1);
m_basePos.setValue(0, 0, 0);
m_basePos.setValue(0, 0, 0);
m_baseForce.setValue(0, 0, 0);
m_baseTorque.setValue(0, 0, 0);
m_isMultiDof = n_links != n_dofs;
}
btMultiBody::~btMultiBody()
@ -164,6 +135,12 @@ void btMultiBody::setupPrismatic(int i,
const btVector3 &parentComToThisComOffset,
bool disableParentCollision)
{
if(m_isMultiDof)
{
m_dofCount += 1;
m_posVarCnt += 1;
}
m_links[i].m_mass = mass;
m_links[i].m_inertia = inertia;
m_links[i].m_parent = parent;
@ -201,6 +178,12 @@ void btMultiBody::setupRevolute(int i,
const btVector3 &thisPivotToThisComOffset,
bool disableParentCollision)
{
if(m_isMultiDof)
{
m_dofCount += 1;
m_posVarCnt += 1;
}
m_links[i].m_mass = mass;
m_links[i].m_inertia = inertia;
m_links[i].m_parent = parent;
@ -240,6 +223,8 @@ void btMultiBody::setupSpherical(int i,
bool disableParentCollision)
{
btAssert(m_isMultiDof);
m_dofCount += 3;
m_posVarCnt += 4;
m_links[i].m_mass = mass;
m_links[i].m_inertia = inertia;
@ -280,6 +265,8 @@ void btMultiBody::setupPlanar(int i,
bool disableParentCollision)
{
btAssert(m_isMultiDof);
m_dofCount += 3;
m_posVarCnt += 3;
m_links[i].m_mass = mass;
m_links[i].m_inertia = inertia;
@ -315,14 +302,12 @@ void btMultiBody::setupPlanar(int i,
}
#endif
void btMultiBody::forceMultiDof()
void btMultiBody::finalizeMultiDof()
{
if(m_isMultiDof) return;
btAssert(m_isMultiDof);
m_isMultiDof = true;
m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices
m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices)
m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices
m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices)
updateLinksDofOffsets();
}

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

@ -49,14 +49,9 @@ public:
btScalar mass, // mass of base
const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal
bool fixedBase, // whether the base is fixed (true) or can move (false)
bool canSleep);
btMultiBody(int n_links, // NOT including the base
int n_dofs, // NOT including 6 floating base dofs
btScalar mass, // mass of base
const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal
bool fixedBase, // whether the base is fixed (true) or can move (false)
bool canSleep);
bool canSleep,
bool multiDof = false
);
~btMultiBody();
@ -356,20 +351,20 @@ public:
// printf("%.4f ", delta_vee[dof]*multiplier);
//printf("\n");
btScalar sum = 0;
for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
{
sum += delta_vee[dof]*multiplier*delta_vee[dof]*multiplier;
}
btScalar l = btSqrt(sum);
//btScalar sum = 0;
//for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
//{
// sum += delta_vee[dof]*multiplier*delta_vee[dof]*multiplier;
//}
//btScalar l = btSqrt(sum);
if (l>m_maxAppliedImpulse)
{
multiplier *= m_maxAppliedImpulse/l;
}
//if (l>m_maxAppliedImpulse)
//{
// multiplier *= m_maxAppliedImpulse/l;
//}
for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
{
for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
{
m_realBuf[dof] += delta_vee[dof] * multiplier;
}
}
@ -480,7 +475,7 @@ public:
}
bool isMultiDof() { return m_isMultiDof; }
void forceMultiDof();
void finalizeMultiDof();
bool __posUpdated;
@ -503,8 +498,6 @@ private:
m_links[bidx].m_dofOffset = dofOffset; m_links[bidx].m_cfgOffset = cfgOffset;
dofOffset += m_links[bidx].m_dofCount; cfgOffset += m_links[bidx].m_posVarCount;
}
m_posVarCnt = cfgOffset;
}
void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const;
@ -529,7 +522,7 @@ private:
//
// realBuf:
// offset size array
// 0 6 + num_links v (base_omega; base_vel; joint_vels)
// 0 6 + num_links v (base_omega; base_vel; joint_vels) MULTIDOF [sysdof x sysdof for D matrices (TOO MUCH!) + pos_delta which is sys-cfg sized]
// 6+num_links num_links D
//
// vectorBuf: