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

switch explicit elastic force

Browse Source
This commit is contained in:
Xuchen Han 2019-07-17 15:58:55 -07:00
parent 2fc376e8f5
commit 7846dd38dd
13 changed files with 193 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/BulletSoftBody/btBackwardEulerObjective.cpp
View File

@ -57,9 +57,6 @@ void btBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) const
{
// add damping matrix
m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b);
// add stiffness matrix when fully implicity
m_lf[i]->addScaledElasticForceDifferential(-m_dt*m_dt, x, b);
}
}
@ -80,3 +77,16 @@ void btBackwardEulerObjective::updateVelocity(const TVStack& dv)
}
}
void btBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack& residual)
{
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)
{
dv[counter] = psb->m_nodes[j].m_im * residual[counter] * 1;
++counter;
}
}
}

36
src/BulletSoftBody/btBackwardEulerObjective.h
View File

@ -76,15 +76,6 @@ class btBackwardEulerObjective
{
public:
using TVStack = btAlignedObjectArray<btVector3>;
// struct DefaultPreconditioner
// {
// void operator()(const TVStack& x, TVStack& b)
// {
// btAssert(b.size() == x.size());
// for (int i = 0; i < b.size(); ++i)
// b[i] = x[i];
// }
// };
btScalar m_dt;
btConjugateGradient<btBackwardEulerObjective> cg;
btDeformableRigidDynamicsWorld* m_world;
@ -102,11 +93,29 @@ public:
void computeResidual(btScalar dt, TVStack& residual) const
{
// gravity is treated explicitly in predictUnconstraintMotion
// add force
// add implicit force
for (int i = 0; i < m_lf.size(); ++i)
{
m_lf[i]->addScaledForce(dt, residual);
m_lf[i]->addScaledImplicitForce(dt, residual);
}
}
void applyExplicitForce(TVStack& force)
{
for (int i = 0; i < m_lf.size(); ++i)
m_lf[i]->addScaledExplicitForce(m_dt, force);
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)
{
btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im;
psb->m_nodes[j].m_v += one_over_mass * force[counter];
force[counter].setZero();
counter++;
}
}
}
@ -117,7 +126,7 @@ public:
{
norm_squared += residual[i].length2();
}
return std::sqrt(norm_squared);
return std::sqrt(norm_squared+SIMD_EPSILON);
}
void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt);
@ -128,6 +137,7 @@ public:
{
projection.update(dv, m_backupVelocity);
}
void initialGuess(TVStack& dv, const TVStack& residual);
void reinitialize(bool nodeUpdated);

16
src/BulletSoftBody/btCGProjection.h
View File

@ -40,13 +40,17 @@ struct Constraint
struct Friction
{
btAlignedObjectArray<bool> m_static;
btAlignedObjectArray<btScalar> m_value;
btAlignedObjectArray<btScalar> m_impulse;
btAlignedObjectArray<btScalar> m_dv;
btAlignedObjectArray<btVector3> m_direction;
btAlignedObjectArray<bool> m_static_prev;
btAlignedObjectArray<btScalar> m_value_prev;
btAlignedObjectArray<btScalar> m_impulse_prev;
btAlignedObjectArray<btScalar> m_dv_prev;
btAlignedObjectArray<btVector3> m_direction_prev;
btAlignedObjectArray<bool> m_released;
btAlignedObjectArray<btVector3> m_accumulated_impulse;
Friction()
{
@ -56,10 +60,14 @@ struct Friction
m_direction.push_back(btVector3(0,0,0));
m_direction_prev.push_back(btVector3(0,0,0));
m_value.push_back(0);
m_value_prev.push_back(0);
m_impulse.push_back(0);
m_impulse_prev.push_back(0);
m_dv.push_back(0);
m_dv_prev.push_back(0);
m_accumulated_impulse.push_back(btVector3(0,0,0));
m_released.push_back(false);
}
};

34
src/BulletSoftBody/btContactProjection.cpp
View File

@ -85,35 +85,41 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
// start friction handling
// copy old data
friction.m_direction_prev[j] = friction.m_direction[j];
friction.m_value_prev[j] = friction.m_value[j];
friction.m_impulse_prev[j] = friction.m_impulse[j];
friction.m_dv_prev[j] = friction.m_dv[j];
friction.m_static_prev[j] = friction.m_static[j];
if (accumulated_normal < 0 && accumulated_tangent.norm() > SIMD_EPSILON)
{
btScalar compare1 = -accumulated_normal*c->m_c3;
btScalar compare2 = accumulated_tangent.norm();
// do not allow switching from static friction to dynamic friction
// it causes cg to explode
if (-accumulated_normal*c->m_c3 < accumulated_tangent.norm() && friction.m_static_prev[j] == false)
if (-accumulated_normal*c->m_c3 < accumulated_tangent.norm() && friction.m_static_prev[j] == false && friction.m_released[j] == false)
{
friction.m_static[j] = false;
friction.m_value[j] = -accumulated_normal*c->m_c3;
friction.m_impulse[j] = -accumulated_normal*c->m_c3;
friction.m_dv[j] = -accumulated_normal*c->m_c3 *c->m_c2/m_dt ;
}
else
{
friction.m_static[j] = true;
friction.m_value[j] = accumulated_tangent.norm();
friction.m_impulse[j] = accumulated_tangent.norm();
friction.m_dv[j] = accumulated_tangent.norm() * c->m_c2/m_dt;
}
const btVector3 tangent_dir = accumulated_tangent.normalized();
impulse_tangent = friction.m_value[j] * tangent_dir;
friction.m_direction[j] = -tangent_dir;
}
else
{
friction.m_released[j] = true;
friction.m_static[j] = false;
friction.m_value[j] = 0;
impulse_tangent.setZero();
friction.m_impulse[j] = 0;
friction.m_dv[j] = 0;
}
friction.m_accumulated_impulse[j] = accumulated_normal * cti.m_normal - friction.m_impulse[j] * friction.m_direction[j];
if (1) // in the same CG solve, the set of constraits doesn't change
// if (dn <= SIMD_EPSILON)
{
@ -128,8 +134,8 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
// the incremental impulse:
// in the normal direction it's the normal component of "impulse"
// in the tangent direction it's the difference between the frictional impulse in the iteration and the previous iteration
impulse = impulse_normal + (friction.m_value_prev[j] * friction.m_direction_prev[j]) - (friction.m_value[j] * friction.m_direction[j]);
impulse = impulse_normal + (friction.m_impulse_prev[j] * friction.m_direction_prev[j]) - (friction.m_impulse[j] * friction.m_direction[j]);
// impulse = impulse_normal;
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
if (rigidCol)
@ -172,7 +178,6 @@ void btContactProjection::setConstraintDirections()
f.push_back(Friction());
f.push_back(Friction());
m_frictions[&(psb->m_nodes[j])] = f;
// no friction constraints for dirichlet
}
}
}
@ -262,11 +267,14 @@ void btContactProjection::setConstraintDirections()
// push in an empty friction
frictions[j].m_direction.push_back(btVector3(0,0,0));
frictions[j].m_direction_prev.push_back(btVector3(0,0,0));
frictions[j].m_value.push_back(0);
frictions[j].m_value_prev.push_back(0);
frictions[j].m_impulse.push_back(0);
frictions[j].m_impulse_prev.push_back(0);
frictions[j].m_dv.push_back(0);
frictions[j].m_dv_prev.push_back(0);
frictions[j].m_static.push_back(false);
frictions[j].m_static_prev.push_back(false);
frictions[j].m_accumulated_impulse.push_back(btVector3(0,0,0));
frictions[j].m_released.push_back(false);
merged = true;
break;
}

53
src/BulletSoftBody/btContactProjection.h
View File

@ -51,13 +51,13 @@ public:
// clear the old friction force
if (friction.m_static_prev[j] == false)
{
x[i] -= friction.m_direction_prev[j] * friction.m_value_prev[j];
x[i] -= friction.m_direction_prev[j] * friction.m_impulse_prev[j];
}
// only add to the rhs if there is no static friction constraint on the node
if (friction.m_static[j] == false && !has_static_constraint)
{
x[i] += friction.m_direction[j] * friction.m_value[j];
x[i] += friction.m_direction[j] * friction.m_impulse[j];
}
}
}
@ -68,6 +68,33 @@ public:
btAssert(free_dir.norm() > SIMD_EPSILON)
free_dir.normalize();
x[i] = x[i].dot(free_dir) * free_dir;
bool has_static_constraint = false;
for (int f = 0; f < 2; ++f)
{
Friction& friction= frictions[f];
for (int j = 0; j < friction.m_static.size(); ++j)
has_static_constraint = has_static_constraint || friction.m_static[j];
}
for (int f = 0; f < 2; ++f)
{
Friction& friction= frictions[f];
for (int j = 0; j < friction.m_direction.size(); ++j)
{
// clear the old friction force
if (friction.m_static_prev[j] == false)
{
x[i] -= friction.m_direction_prev[j] * friction.m_impulse_prev[j];
}
// only add to the rhs if there is no static friction constraint on the node
if (friction.m_static[j] == false && !has_static_constraint)
{
x[i] += friction.m_direction[j] * friction.m_impulse[j];
}
}
}
}
else
x[i].setZero();
@ -97,12 +124,12 @@ public:
// clear the old constraint
if (friction.m_static_prev[j] == true)
{
x[i] -= friction.m_direction_prev[j] * friction.m_value_prev[j];
x[i] -= friction.m_direction_prev[j] * friction.m_dv_prev[j];
}
// add the new constraint
if (friction.m_static[j] == true)
{
x[i] += friction.m_direction[j] * friction.m_value[j];
x[i] += friction.m_direction[j] * friction.m_dv[j];
}
}
}
@ -120,6 +147,24 @@ public:
x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k];
}
}
for (int f = 0; f < 2; ++f)
{
const Friction& friction= frictions[f];
for (int j = 0; j < friction.m_direction.size(); ++j)
{
// clear the old constraint
if (friction.m_static_prev[j] == true)
{
x[i] -= friction.m_direction_prev[j] * friction.m_dv_prev[j];
}
// add the new constraint
if (friction.m_static[j] == true)
{
x[i] += friction.m_direction[j] * friction.m_dv[j];
}
}
}
}
else
{

11
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@ -118,7 +118,15 @@ void btDeformableBodySolver::solveConstraints(float solverdt)
m_dt = solverdt;
bool nodeUpdated = updateNodes();
reinitialize(nodeUpdated);
// apply explicit force
m_objective->applyExplicitForce(m_residual);
// remove contact constraints with separating velocity
setConstraintDirections();
backupVelocity();
for (int i = 0; i < m_solveIterations; ++i)
{
m_objective->computeResidual(solverdt, m_residual);
@ -143,9 +151,6 @@ void btDeformableBodySolver::reinitialize(bool nodeUpdated)
m_residual[i].setZero();
}
m_objective->reinitialize(nodeUpdated);
// remove contact constraints with separating velocity
setConstraintDirections();
}
void btDeformableBodySolver::setConstraintDirections()

14
src/BulletSoftBody/btDeformableBodySolver.h
View File

@ -74,20 +74,6 @@ public:
void postStabilize();
void moveTempVelocity(btScalar dt, const TVStack& f)
{
size_t 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)
{
auto& node = psb->m_nodes[j];
node.m_v += node.m_im * dt * f[counter++];
}
}
}
void reinitialize(bool nodeUpdated);
void setConstraintDirections();

7
src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
View File

@ -12,6 +12,7 @@
void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
{
m_internalTime += timeStep;
// Let the solver grab the soft bodies and if necessary optimize for it
m_deformableBodySolver->optimize(m_softBodies);
@ -54,7 +55,6 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS
// gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again
// so that 1) gravity is applied to velocity before constraint solve and 2) gravity is applied in each substep
// when there are multiple substeps
clearForces();
clearMultiBodyForces();
btMultiBodyDynamicsWorld::applyGravity();
@ -69,9 +69,14 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS
clearForces();
clearMultiBodyForces();
for (int i = 0; i < before_solver_callbacks.size(); ++i)
before_solver_callbacks[i](m_internalTime, this);
///solve deformable bodies constraints
solveDeformableBodiesConstraints(timeStep);
//integrate transforms
btMultiBodyDynamicsWorld::integrateTransforms(timeStep);

9
src/BulletSoftBody/btDeformableRigidDynamicsWorld.h
View File

@ -21,7 +21,7 @@
#include "btMassSpring.h"
#include "btDeformableBodySolver.h"
#include "btSoftBodyHelpers.h"
#include <functional>
typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
class btDeformableBodySolver;
@ -40,6 +40,7 @@ class btDeformableRigidDynamicsWorld : public btMultiBodyDynamicsWorld
bool m_drawClusterTree;
btSoftBodyWorldInfo m_sbi;
bool m_ownsSolver;
btScalar m_internalTime;
protected:
virtual void internalSingleStepSimulation(btScalar timeStep);
@ -67,8 +68,9 @@ public:
m_sbi.m_gravity.setValue(0, -10, 0);
m_sbi.m_sparsesdf.Initialize();
m_internalTime = 0.0;
}
btAlignedObjectArray<std::function<void(btScalar, btDeformableRigidDynamicsWorld*)> > before_solver_callbacks;
virtual ~btDeformableRigidDynamicsWorld()
{
}
@ -89,9 +91,6 @@ public:
}
virtual void predictUnconstraintMotion(btScalar timeStep);
// virtual void internalStepSingleStepSimulation(btScalar timeStep);
// virtual void solveDeformableBodiesConstraints(btScalar timeStep);
virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter);

4
src/BulletSoftBody/btLagrangianForce.h
View File

@ -25,12 +25,14 @@ public:
virtual ~btLagrangianForce(){}
virtual void addScaledForce(btScalar scale, TVStack& force) = 0;
virtual void addScaledImplicitForce(btScalar scale, TVStack& force) = 0;
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0;
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0;
virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0;
virtual void reinitialize(bool nodeUpdated)
{
if (nodeUpdated)

55
src/BulletSoftBody/btMassSpring.h
View File

@ -19,7 +19,17 @@ public:
}
virtual void addScaledForce(btScalar scale, TVStack& force)
virtual void addScaledImplicitForce(btScalar scale, TVStack& force)
{
addScaledDampingForce(scale, force);
}
virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
{
addScaledElasticForce(scale, force);
}
virtual void addScaledDampingForce(btScalar scale, TVStack& force)
{
int numNodes = getNumNodes();
btAssert(numNodes == force.size())
@ -31,7 +41,32 @@ public:
const auto& link = psb->m_links[j];
const auto node1 = link.m_n[0];
const auto node2 = link.m_n[1];
btScalar kLST = link.Feature::m_material->m_kLST; // this is probly wrong, TODO: figure out how to get stiffness
size_t id1 = m_indices[node1];
size_t id2 = m_indices[node2];
// damping force
btVector3 v_diff = (node2->m_v - node1->m_v);
btScalar k_damp = psb->m_dampingCoefficient;
btVector3 scaled_force = scale * v_diff * k_damp;
force[id1] += scaled_force;
force[id2] -= scaled_force;
}
}
}
virtual void addScaledElasticForce(btScalar scale, TVStack& force)
{
int numNodes = getNumNodes();
btAssert(numNodes == force.size())
for (int i = 0; i < m_softBodies.size(); ++i)
{
const btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_links.size(); ++j)
{
const auto& link = psb->m_links[j];
const auto node1 = link.m_n[0];
const auto node2 = link.m_n[1];
btScalar kLST = link.Feature::m_material->m_kLST;
btScalar r = link.m_rl;
size_t id1 = m_indices[node1];
size_t id2 = m_indices[node2];
@ -39,21 +74,14 @@ public:
// elastic force
// fully implicit
btVector3 dir = (node2->m_x - node1->m_x);
// btVector3 dir = (node2->m_x - node1->m_x);
// explicit elastic force
// btVector3 dir = (node2->m_q - node1->m_q);
btVector3 dir = (node2->m_q - node1->m_q);
btVector3 dir_normalized = dir.normalized();
btVector3 scaled_force = scale * kLST * (dir - dir_normalized * r);
force[id1] += scaled_force;
force[id2] -= scaled_force;
// damping force
btVector3 v_diff = (node2->m_v - node1->m_v);
btScalar k_damp = psb->m_dampingCoefficient; // TODO: FIX THIS HACK and set k_damp properly
scaled_force = scale * v_diff * k_damp;
force[id1] += scaled_force;
force[id2] -= scaled_force;
}
}
}
@ -64,7 +92,7 @@ public:
btAssert(numNodes == dx.size());
btAssert(numNodes == df.size());
// implicit elastic force
// implicit elastic force differential
for (int i = 0; i < m_softBodies.size(); ++i)
{
const btSoftBody* psb = m_softBodies[i];
@ -83,9 +111,10 @@ public:
}
}
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
{
// implicity damping force
// implicit damping force differential
for (int i = 0; i < m_softBodies.size(); ++i)
{
const btSoftBody* psb = m_softBodies[i];

12
src/BulletSoftBody/btSoftBody.cpp
View File

@ -1782,7 +1782,7 @@ void btSoftBody::predictMotion(btScalar dt)
m_sst.radmrg = getCollisionShape()->getMargin();
m_sst.updmrg = m_sst.radmrg * (btScalar)0.25;
/* Forces */
addVelocity(m_worldInfo->m_gravity * m_sst.sdt);
addVelocity(m_worldInfo->m_gravity * m_sst.sdt);
applyForces();
/* Integrate */
for (i = 0, ni = m_nodes.size(); i < ni; ++i)
@ -1805,7 +1805,7 @@ void btSoftBody::predictMotion(btScalar dt)
}
}
}
n.m_v += deltaV;
n.m_v += deltaV;
n.m_x += n.m_v * m_sst.sdt;
n.m_f = btVector3(0, 0, 0);
}
@ -2775,6 +2775,14 @@ void btSoftBody::dampClusters()
}
}
void btSoftBody::setSpringStiffness(btScalar k)
{
for (int i = 0; i < m_links.size(); ++i)
{
m_links[i].Feature::m_material->m_kLST = k;
}
}
//
void btSoftBody::Joint::Prepare(btScalar dt, int)
{

1
src/BulletSoftBody/btSoftBody.h
View File

@ -1011,6 +1011,7 @@ public:
void solveClusters(btScalar sor);
void applyClusters(bool drift);
void dampClusters();
void setSpringStiffness(btScalar k);
void applyForces();
static void PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti);
static void PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti);