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

fixed gravity issue in rigid body and deformable body contact solve

Browse Source
This commit is contained in:
Xuchen Han
2019-07-07 14:31:19 -07:00
parent b8997c36b2
commit 786b0436ec
7 changed files with 72 additions and 121 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/BulletSoftBody/btBackwardEulerObjective.h
View File

@@ -40,7 +40,7 @@ public:
: cg(20)
, m_softBodies(softBodies)
, precondition(DefaultPreconditioner())
, projection(m_softBodies, backup_v)
, projection(m_softBodies)
{
// TODO: this should really be specified in initialization instead of here
btMassSpring* mass_spring = new btMassSpring(m_softBodies);

4
src/BulletSoftBody/btCGProjection.h
View File

@@ -23,11 +23,9 @@ public:
std::unordered_map<btSoftBody::Node *, size_t> m_indices;
TVArrayStack m_constrainedDirections;
TArrayStack m_constrainedValues;
const TVStack& m_backupVelocity;
btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v)
btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies)
: m_softBodies(softBodies)
, m_backupVelocity(backup_v)
{
}

19
src/BulletSoftBody/btContactProjection.cpp
View File

@@ -9,17 +9,6 @@
#include "btDeformableRigidDynamicsWorld.h"
void btContactProjection::update(btScalar dt, const TVStack& dv)
{
size_t counter = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_v = m_backupVelocity[counter] + dv[counter];
++counter;
}
}
///solve rigid body constraints
m_world->btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::solveConstraints(m_world->getSolverInfo());
@@ -31,7 +20,7 @@ void btContactProjection::update(btScalar dt, const TVStack& dv)
}
// Set dirichlet constraints
counter = 0;
size_t counter = 0;
for (int i = 0; i < m_softBodies.size(); ++i)
{
const btSoftBody* psb = m_softBodies[i];
@@ -76,7 +65,7 @@ void btContactProjection::update(btScalar dt, const TVStack& dv)
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1) + btVector3(0,-10,0)*dt) * dt : btVector3(0, 0, 0);
va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * dt : btVector3(0, 0, 0);
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
@@ -114,11 +103,9 @@ void btContactProjection::update(btScalar dt, const TVStack& dv)
// const btVector3 impulse = c.m_c0 * ((vr - (fv * c.m_c3)));
const btVector3 impulse = c.m_c0 * ((vr - (fv * c.m_c3))+ (cti.m_normal * (dp * c.m_c4)));
//c.m_node->m_v -= impulse * c.m_c2 / dt;
// TODO: only contact is considered here, add friction later
btVector3 normal = cti.m_normal.normalized();
btVector3 diff = c.m_node->m_v - m_backupVelocity[m_indices[c.m_node]];
btVector3 dv = -impulse * c.m_c2/dt + diff;
btVector3 dv = -impulse * c.m_c2/dt;
btScalar dvn = dv.dot(normal);
m_constrainedDirections[m_indices[c.m_node]].push_back(normal);
m_constrainedValues[m_indices[c.m_node]].push_back(dvn);

4
src/BulletSoftBody/btContactProjection.h
View File

@@ -15,8 +15,8 @@
class btContactProjection : public btCGProjection
{
public:
btContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btAlignedObjectArray<btVector3>& backup_v)
: btCGProjection(softBodies, backup_v)
btContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies)
: btCGProjection(softBodies)
{
}

24
src/BulletSoftBody/btDeformableBodySolver.h
View File

@@ -82,13 +82,11 @@ public:
{
bool nodeUpdated = updateNodes();
reinitialize(nodeUpdated);
backupVelocity();
for (int i = 0; i < m_solveIterations; ++i)
{
// only need to advect x here if elastic force is implicit
// prepareSolve(solverdt);
m_objective.computeResidual(solverdt, m_residual);
moveTempVelocity(solverdt, m_residual);
m_objective.computeStep(m_dv, m_residual, solverdt);
updateVelocity();
@@ -116,7 +114,6 @@ public:
{
m_dv.resize(m_numNodes);
m_residual.resize(m_numNodes);
m_backupVelocity.resize(m_numNodes);
}
for (int i = 0; i < m_dv.size(); ++i)
@@ -150,11 +147,6 @@ public:
auto& node = psb->m_nodes[j];
// node.m_x += dt * m_dv[counter++];
node.m_x += dt * node.m_v;
if (j == 4)
{
std::cout << "x " << psb->m_nodes[j].m_x.getY() << std::endl;
std::cout << "v " << psb->m_nodes[j].m_v.getY() << std::endl;
}
}
}
}
@@ -168,27 +160,13 @@ public:
btSoftBody* psb = m_softBodySet[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter];
psb->m_nodes[j].m_v += m_dv[counter];
++counter;
}
}
}
void backupVelocity()
{
// serial implementation
int counter = 0;
for (int i = 0; i < m_softBodySet.size(); ++i)
{
btSoftBody* psb = m_softBodySet[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
m_backupVelocity[counter++] = psb->m_nodes[j].m_v;
}
}
}
bool updateNodes()
{
int numNodes = 0;

15
src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
View File

@@ -53,6 +53,14 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS
// btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::internalSingleStepSimulation(timeStep);
// incorporate gravity into velocity and clear force
for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i)
{
btRigidBody* rb = m_nonStaticRigidBodies[i];
rb->integrateVelocities(timeStep);
}
clearForces();
///solve deformable bodies constraints
solveDeformableBodiesConstraints(timeStep);
@@ -67,12 +75,7 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS
///update soft bodies
m_deformableBodySolver->updateSoftBodies();
for (int i = 0; i < m_nonStaticRigidBodies.size(); i++)
{
btRigidBody* body = m_nonStaticRigidBodies[i];
std::cout << "rb v = " << body->getLinearVelocity().getY() << std::endl;
}
// End solver-wise simulation step
// ///////////////////////////////
}