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

contact solve for newton

Browse Source
This commit is contained in:
Xuchen Han 2019-08-28 10:01:14 -07:00
parent 5826492020
commit 7d1b93cc17
9 changed files with 63 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
data/single_tet.vtk Normal file
View File

@ -0,0 +1,15 @@
# vtk DataFile Version 2.0
ball_, Created by Gmsh
ASCII
DATASET UNSTRUCTURED_GRID
POINTS 4 double
0 0 0
1 0 0
0 1 0
0 0 1
CELLS 1 1
4 0 1 2 3
CELL_TYPES 1
10

4
examples/DeformableDemo/DeformableRigid.cpp
View File

@ -234,11 +234,11 @@ void DeformableRigid::initPhysics()
psb->setTotalMass(1);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
psb->m_cfg.kDF = 1;
psb->m_cfg.kDF = 2;
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
getDeformableDynamicsWorld()->addSoftBody(psb);
btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2,0.01, false);
btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2,0.1, true);
getDeformableDynamicsWorld()->addForce(psb, mass_spring);
m_forces.push_back(mass_spring);

24
examples/DeformableDemo/GraspDeformable.cpp
View File

@ -325,7 +325,8 @@ void GraspDeformable::initPhysics()
{
char relative_path[1024];
// b3FileUtils::findFile("banana.vtk", relative_path, 1024);
b3FileUtils::findFile("ball.vtk", relative_path, 1024);
b3FileUtils::findFile("ball.vtk", relative_path, 1024);
// b3FileUtils::findFile("single_tet.vtk", relative_path, 1024);
// b3FileUtils::findFile("tube.vtk", relative_path, 1024);
// b3FileUtils::findFile("torus.vtk", relative_path, 1024);
// b3FileUtils::findFile("paper_roll.vtk", relative_path, 1024);
@ -343,21 +344,16 @@ void GraspDeformable::initPhysics()
psb->scale(btVector3(.25, .25, .25));
// psb->scale(btVector3(.3, .3, .3)); // for tube, torus, boot
// psb->scale(btVector3(1, 1, 1)); // for ditto
// psb->translate(btVector3(0, 0, 2)); for boot
psb->getCollisionShape()->setMargin(0.01);
psb->translate(btVector3(.25, 0, 0.4));
psb->getCollisionShape()->setMargin(0.02);
psb->setTotalMass(.1);
psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
psb->m_cfg.kCHR = 1; // collision hardness with rigid body
psb->m_cfg.kDF = 2;
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
getDeformableDynamicsWorld()->addSoftBody(psb);
psb->getWorldInfo()->m_maxDisplacement = .1f;
// nonlinear damping
// getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(1,0.04, true));
// getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
// getDeformableDynamicsWorld()->addForce(psb, new btDeformableCorotatedForce(0,6));
btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(.0,0.04, true);
btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(.0,.04, true);
getDeformableDynamicsWorld()->addForce(psb, mass_spring);
m_forces.push_back(mass_spring);
@ -365,7 +361,7 @@ void GraspDeformable::initPhysics()
getDeformableDynamicsWorld()->addForce(psb, gravity_force);
m_forces.push_back(gravity_force);
btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(2,10);
btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(5,10);
getDeformableDynamicsWorld()->addForce(psb, neohookean);
m_forces.push_back(neohookean);
}
@ -415,8 +411,8 @@ void GraspDeformable::initPhysics()
{
SliderParams slider("Closing velocity", &sGripperClosingTargetVelocity);
slider.m_minVal = -.1;
slider.m_maxVal = .1;
slider.m_minVal = -.3;
slider.m_maxVal = .3;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
@ -470,8 +466,8 @@ btMultiBody* GraspDeformable::createFeatherstoneMultiBody(btMultiBodyDynamicsWor
{
//init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = 1.f;
float linkMass = 1.f;
float baseMass = 100.f;
float linkMass = 100.f;
int numLinks = 2;
if (baseMass)

2
src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
View File

@ -83,7 +83,7 @@ void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b)
void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv)
{
// only the velocity of the constrained nodes needs to be updated during CG solve
// only the velocity of the constrained nodes needs to be updated during contact solve
for (int i = 0; i < projection.m_constraints.size(); ++i)
{
int index = projection.m_constraints.getKeyAtIndex(i).getUid1();

19
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@ -67,7 +67,7 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
{
break;
}
// m_objective->applyDynamicFriction(m_residual);
m_objective->applyDynamicFriction(m_residual);
computeStep(m_ddv, m_residual);
updateDv();
for (int j = 0; j < m_numNodes; ++j)
@ -189,6 +189,20 @@ void btDeformableBodySolver::backupVelocity()
}
}
void btDeformableBodySolver::backupVn()
{
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_dv[counter] += m_backupVelocity[counter] - psb->m_nodes[j].m_vn;
m_backupVelocity[counter++] = psb->m_nodes[j].m_vn;
}
}
}
void btDeformableBodySolver::revertVelocity()
{
int counter = 0;
@ -246,8 +260,7 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d
for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i)
{
btSoftBody::Node& n = psb->m_nodes[i];
n.m_q = n.m_x;
n.m_q += n.m_v * dt;
n.m_q = n.m_x + n.m_v * dt;
}
/* Bounds */
psb->updateBounds();

1
src/BulletSoftBody/btDeformableBodySolver.h
View File

@ -75,6 +75,7 @@ public:
void predictDeformableMotion(btSoftBody* psb, btScalar dt);
void backupVelocity();
void backupVn();
void revertVelocity();
void updateVelocity();

29
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
View File

@ -168,6 +168,7 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
}
node.m_x = node.m_x + timeStep * node.m_v;
node.m_q = node.m_x;
node.m_vn = node.m_v;
}
}
m_deformableBodySolver->revertVelocity();
@ -175,11 +176,8 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
{
if (!m_implicit)
{
// save v_{n+1}^* velocity after explicit forces
m_deformableBodySolver->backupVelocity();
}
// save v_{n+1}^* velocity after explicit forces
m_deformableBodySolver->backupVelocity();
// set up constraints among multibodies and between multibodies and deformable bodies
setupConstraints();
@ -187,11 +185,13 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
if (m_implicit)
{
// revert to v_n after collisions are registered
m_deformableBodySolver->revertVelocity();
// todo @xuchenhan : think about whether contact solve should be done with v_n velocity or v_{n+1}^* velocity. It's using v_n for implicit and v_{n+1}^* for non-implicit now.
// at this point dv = v_{n+1} - v_{n+1}^*
// modify dv such that dv = v_{n+1} - v_n
// modify m_backupVelocity so that it stores v_n instead of v_{n+1}^* as needed by Newton
m_deformableBodySolver->backupVn();
}
// At this point, dv is golden for nodes in contact
// At this point, dv should be golden for nodes in contact
m_deformableBodySolver->solveDeformableConstraints(timeStep);
}
@ -211,7 +211,6 @@ void btDeformableMultiBodyDynamicsWorld::setupConstraints()
// build islands
m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld());
// write the constraint information of each island to the callback, and also setup the constraints in solver
}
}
@ -317,11 +316,11 @@ void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep)
m_internalTime += timeStep;
m_deformableBodySolver->setImplicit(m_implicit);
m_deformableBodySolver->reinitialize(m_softBodies, timeStep);
if (m_implicit)
{
// backup v_n velocity
m_deformableBodySolver->backupVelocity();
}
// if (m_implicit)
// {
// // todo: backup v_n velocity somewhere else
// m_deformableBodySolver->backupVelocity();
// }
btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo();
dispatchInfo.m_timeStep = timeStep;
dispatchInfo.m_stepCount = 0;

4
src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h
View File

@ -73,7 +73,7 @@ public:
m_sbi.m_broadphase = pairCache;
m_sbi.m_dispatcher = dispatcher;
m_sbi.m_sparsesdf.Initialize();
m_sbi.m_sparsesdf.setDefaultVoxelsz(0.0025);
m_sbi.m_sparsesdf.setDefaultVoxelsz(0.025);
m_sbi.m_sparsesdf.Reset();
m_sbi.air_density = (btScalar)1.2;
@ -82,7 +82,7 @@ public:
m_sbi.water_normal = btVector3(0, 0, 0);
m_sbi.m_gravity.setValue(0, -10, 0);
m_internalTime = 0.0;
m_implicit = false;
m_implicit = true;
}
void setSolverCallback(btSolverCallback cb)

3
src/BulletSoftBody/btSoftBody.h
View File

@ -251,8 +251,9 @@ public:
struct Node : Feature
{
btVector3 m_x; // Position
btVector3 m_q; // Previous step position
btVector3 m_q; // Previous step position/Test position
btVector3 m_v; // Velocity
btVector3 m_vn; // Previous step velocity
btVector3 m_f; // Force accumulator
btVector3 m_n; // Normal
btScalar m_im; // 1/mass