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

125
examples/DeformableDemo/DeformableDemo.cpp
View File

@@ -59,16 +59,45 @@ public:
void resetCamera()
{
// float dist = 30;
// float pitch = -14;
// float yaw = 0;
// float targetPos[3] = {0, 0, 0};
float dist = 45;
float dist = 15;
float pitch = -45;
float yaw = 100;
float targetPos[3] = {0,0, 0};
float targetPos[3] = {0, -5, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
void Ctor_RbUpStack(int count)
{
float mass = 1;
btCompoundShape* cylinderCompound = new btCompoundShape;
btCollisionShape* cylinderShape = new btCylinderShapeX(btVector3(2, .5, .5));
btCollisionShape* boxShape = new btBoxShape(btVector3(2, .5, .5));
btTransform localTransform;
localTransform.setIdentity();
cylinderCompound->addChildShape(localTransform, boxShape);
btQuaternion orn(SIMD_HALF_PI, 0, 0);
localTransform.setRotation(orn);
// localTransform.setOrigin(btVector3(1,1,1));
cylinderCompound->addChildShape(localTransform, cylinderShape);
btCollisionShape* shape[] = {
new btBoxShape(btVector3(1, 1, 1)),
cylinderCompound,
new btSphereShape(0.75)
};
static const int nshapes = sizeof(shape) / sizeof(shape[0]);
for (int i = 0; i < count; ++i)
{
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0, 3 + 3 * i, 0));
createRigidBody(mass, startTransform, shape[i % nshapes]);
}
}
virtual const btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() const
{
///just make it a btSoftRigidDynamicsWorld please
@@ -105,7 +134,6 @@ void DeformableDemo::initPhysics()
m_guiHelper->setUpAxis(1);
///collision configuration contains default setup for memory, collision setup
// m_collisionConfiguration = new btDefaultCollisionConfiguration();
m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
@@ -119,22 +147,22 @@ void DeformableDemo::initPhysics()
m_solver = sol;
m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration, deformableBodySolver);
deformableBodySolver->setWorld(getDeformableDynamicsWorld());
// m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality
m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
getDeformableDynamicsWorld()->getSoftDynamicsWorld()->getWorldInfo().m_gravity.setValue(0, -10, 0);
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
{
///create a few basic rigid bodies
// btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(10.), btScalar(5.), btScalar(25.)));
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.), btScalar(25.), btScalar(50.)));
///create a ground
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(25.), btScalar(150.)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, -30, 0));
// groundTransform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI * 0.03));
groundTransform.setOrigin(btVector3(0, -50, 0));
groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.0));
//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
btScalar mass(0.);
@@ -151,76 +179,30 @@ void DeformableDemo::initPhysics()
btRigidBody* body = new btRigidBody(rbInfo);
body->setFriction(.5);
//add the body to the dynamics world
//add the ground to the dynamics world
m_dynamicsWorld->addRigidBody(body);
}
//
// {
// ///create a few basic rigid bodies
// btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(100.), btScalar(100.), btScalar(50.)));
//
// m_collisionShapes.push_back(groundShape);
//
// btTransform groundTransform;
// groundTransform.setIdentity();
// groundTransform.setOrigin(btVector3(0, 0, -54));
// //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
// btScalar mass(0.);
//
// //rigidbody is dynamic if and only if mass is non zero, otherwise static
// bool isDynamic = (mass != 0.f);
//
// btVector3 localInertia(0, 0, 0);
// if (isDynamic)
// groundShape->calculateLocalInertia(mass, localInertia);
//
// //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
// btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
// btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
// btRigidBody* body = new btRigidBody(rbInfo);
// body->setFriction(.1);
// //add the body to the dynamics world
// m_dynamicsWorld->addRigidBody(body);
// }
//
// {
// // add a simple deformable body
// const btVector3 s(3,2,1); // side length
// const btVector3 p(0,30,0); // origin;
// const btVector3 h = s * 0.5;
// const btVector3 c[] = {p + h * btVector3(-1, -1, -1),
// p + h * btVector3(+1, -1, -1),
// p + h * btVector3(-1, +1, -1),
// p + h * btVector3(+1, +1, -1),
// p + h * btVector3(-1, -1, +1),
// p + h * btVector3(+1, -1, +1),
// p + h * btVector3(-1, +1, +1),
// p + h * btVector3(+1, +1, +1)};
// btSoftBody* psb = btSoftBodyHelpers::CreateFromConvexHull(getDeformableDynamicsWorld()->getSoftDynamicsWorld()->getWorldInfo(), c, 8);
// psb->generateBendingConstraints(2);
// psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS;
// psb->setTotalMass(150);
// getDeformableDynamicsWorld()->addSoftBody(psb);
// }
// create a piece of cloth
{
const btScalar s = 8;
const btScalar s = 4;
btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getSoftDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s),
btVector3(+s, 0, -s),
btVector3(-s, 0, +s),
btVector3(+s, 0, +s),
10, 10,
// 31,31,
// 3, 3,
20,20,
1 + 2 + 4 + 8, true);
// 0, true);
psb->getCollisionShape()->setMargin(0.5);
// btSoftBody::Material* pm = psb->appendMaterial();
// pm->m_kLST = 0.4 * 1000;
// pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
psb->getCollisionShape()->setMargin(0.25);
psb->generateBendingConstraints(2);
psb->setTotalMass(1);
psb->setDampingCoefficient(0.01);
psb->setTotalMass(2);
psb->setDampingCoefficient(0.02);
getDeformableDynamicsWorld()->addSoftBody(psb);
// add a few rigid bodies
Ctor_RbUpStack(10);
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
@@ -263,7 +245,10 @@ void DeformableDemo::exitPhysics()
}
class CommonExampleInterface* DeformableCreateFunc(struct CommonExampleOptions& options)
{
return new DeformableDemo(options.m_guiHelper);
}

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;

13
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);
@@ -68,11 +76,6 @@ 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
// ///////////////////////////////
}