Scarfski/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3.git synced 2026-06-08 08:13:55 +00:00
Code Issues Packages Projects Releases Wiki Activity

add implicit point contact for corotated linear elasticity objects

Browse Source
This commit is contained in:
Xuchen Han
2020-06-19 14:49:58 -07:00
parent 92f2f56b3d
commit 395f8c6117
8 changed files with 158 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

82
src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
View File

@@ -49,6 +49,17 @@ void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated, btScalar
{
m_lf[i]->reinitialize(nodeUpdated);
}
btMatrix3x3 I;
I.setIdentity();
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
if (psb->m_nodes[j].m_im > 0)
psb->m_nodes[j].m_effectiveMass = I * (1.0 / psb->m_nodes[j].m_im);
}
}
m_projection.reinitialize(nodeUpdated);
// m_preconditioner->reinitialize(nodeUpdated);
}
@@ -136,10 +147,23 @@ void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero
counter += psb->m_nodes.size();
continue;
}
for (int j = 0; j < psb->m_nodes.size(); ++j)
if (m_implicit)
{
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++];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
if (psb->m_nodes[j].m_im != 0)
{
psb->m_nodes[j].m_v += psb->m_nodes[j].m_effectiveMass_inv * force[counter++];
}
}
}
else
{
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++];
}
}
}
if (setZero)
@@ -193,10 +217,56 @@ void btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force)
{
m_softBodies[i]->advanceDeformation();
}
for (int i = 0; i < m_lf.size(); ++i)
if (m_implicit)
{
m_lf[i]->addScaledExplicitForce(m_dt, force);
// apply forces except gravity force
btVector3 gravity;
for (int i = 0; i < m_lf.size(); ++i)
{
if (m_lf[i]->getForceType() == BT_GRAVITY_FORCE)
{
gravity = dynamic_cast<btDeformableGravityForce*>(m_lf[i])->m_gravity;
}
else
{
m_lf[i]->addScaledExplicitForce(m_dt, force);
}
}
for (int i = 0; i < m_lf.size(); ++i)
{
m_lf[i]->addScaledHessian(m_dt);
}
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (psb->isActive())
{
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
// add gravity explicitly
psb->m_nodes[j].m_v += m_dt * psb->m_gravityFactor * gravity;
}
}
}
}
else
{
for (int i = 0; i < m_lf.size(); ++i)
{
m_lf[i]->addScaledExplicitForce(m_dt, force);
}
}
// calculate inverse mass matrix for all nodes
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (psb->isActive())
{
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_effectiveMass_inv = psb->m_nodes[j].m_effectiveMass.inverse();
}
}
}
applyForce(force, true);
}

12
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@@ -390,7 +390,19 @@ bool btDeformableBodySolver::updateNodes()
void btDeformableBodySolver::predictMotion(btScalar solverdt)
{
// apply explicit forces to velocity
for (int i = 0; i<m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (psb->isActive())
{
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + psb->m_nodes[j].m_v * solverdt;
}
}
}
m_objective->applyExplicitForce(m_residual);
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody *psb = m_softBodies[i];

14
src/BulletSoftBody/btDeformableContactConstraint.cpp
View File

@@ -286,15 +286,14 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv
btVector3 impulse_tangent = impulse - impulse_normal;
if (dn > 0)
{
m_binding = false;
return 0;
}
m_binding = true;
btScalar residualSquare = dn*dn;
btVector3 old_total_tangent_dv = m_total_tangent_dv;
// m_c2 is the inverse mass of the deformable node/face
m_total_normal_dv -= impulse_normal * m_contact->m_c2;
m_total_tangent_dv -= impulse_tangent * m_contact->m_c2;
// m_c5 is the inverse mass of the deformable node/face
m_total_normal_dv -= m_contact->m_c5 * impulse_normal;
m_total_tangent_dv -= m_contact->m_c5 * impulse_tangent;
if (m_total_normal_dv.dot(cti.m_normal) < 0)
{
@@ -317,7 +316,8 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv
{
m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_c3;
}
impulse_tangent = -btScalar(1)/m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv);
// impulse_tangent = -btScalar(1)/m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv);
impulse_tangent = m_contact->m_c5.inverse() * (old_total_tangent_dv - m_total_tangent_dv);
}
else
{
@@ -445,14 +445,14 @@ btVector3 btDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node*
void btDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impulse)
{
const btSoftBody::DeformableNodeRigidContact* contact = getContact();
btVector3 dv = impulse * contact->m_c2;
btVector3 dv = contact->m_c5 * impulse;
contact->m_node->m_v -= dv;
}
void btDeformableNodeRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
{
const btSoftBody::DeformableNodeRigidContact* contact = getContact();
btVector3 dv = impulse * contact->m_c2;
btVector3 dv = contact->m_c5 * impulse;
contact->m_node->m_splitv -= dv;
}

2
src/BulletSoftBody/btDeformableLagrangianForce.h
View File

@@ -66,6 +66,8 @@ public:
// add all damping forces
virtual void addScaledDampingForce(btScalar scale, TVStack& force) = 0;
virtual void addScaledHessian(btScalar scale){}
virtual btDeformableLagrangianForceType getForceType() = 0;
virtual void reinitialize(bool nodeUpdated)

41
src/BulletSoftBody/btDeformableLinearElasticityForce.h
View File

@@ -18,6 +18,7 @@
#include "btDeformableLagrangianForce.h"
#include "LinearMath/btQuickprof.h"
#include "btSoftBodyInternals.h"
#define TETRA_FLAT_THRESHOLD 0.01
class btDeformableLinearElasticityForce : public btDeformableLagrangianForce
{
@@ -413,6 +414,46 @@ public:
dP = (dF + dF.transpose()) * mu_damp + btMatrix3x3::getIdentity() * lambda_damp * trace;
}
virtual void addScaledHessian(btScalar scale)
{
btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
if (!psb->isActive())
{
continue;
}
for (int j = 0; j < psb->m_tetras.size(); ++j)
{
btSoftBody::Tetra& tetra = psb->m_tetras[j];
btMatrix3x3 P;
firstPiola(psb->m_tetraScratches[j],P); // make sure scratch is evaluated at x_n + dt * vn
btMatrix3x3 force_on_node123 = psb->m_tetraScratches[j].m_corotation * P * tetra.m_Dm_inverse.transpose();
btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col;
btSoftBody::Node* node0 = tetra.m_n[0];
btSoftBody::Node* node1 = tetra.m_n[1];
btSoftBody::Node* node2 = tetra.m_n[2];
btSoftBody::Node* node3 = tetra.m_n[3];
btScalar scale1 = scale * (scale + m_damping_beta) * tetra.m_element_measure; // stiff and stiffness-damping terms;
node0->m_effectiveMass += OuterProduct(force_on_node0,force_on_node0) * scale1;
node1->m_effectiveMass += OuterProduct(force_on_node123.getColumn(0),force_on_node123.getColumn(0)) * scale1;
node2->m_effectiveMass += OuterProduct(force_on_node123.getColumn(1),force_on_node123.getColumn(1)) * scale1;
node3->m_effectiveMass += OuterProduct(force_on_node123.getColumn(2),force_on_node123.getColumn(2)) * scale1;
}
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
btSoftBody::Node& node = psb->m_nodes[j];
if (node.m_im > 0)
{
btMatrix3x3 I;
I.setIdentity();
node.m_effectiveMass += I * (scale * (1.0/node.m_im) * m_damping_alpha);
}
}
}
}
virtual btDeformableLagrangianForceType getForceType()
{
return BT_LINEAR_ELASTICITY_FORCE;

2
src/BulletSoftBody/btSoftBody.cpp
View File

@@ -902,6 +902,7 @@ void btSoftBody::setVelocity(const btVector3& velocity)
if (n.m_im > 0)
{
n.m_v = velocity;
n.m_vn = velocity;
}
}
}
@@ -2775,6 +2776,7 @@ bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWr
shp,
nrm,
margin);
if (!predict)
{
cti.m_colObj = colObjWrap->getCollisionObject();

3
src/BulletSoftBody/btSoftBody.h
View File

@@ -273,6 +273,8 @@ public:
int m_battach : 1; // Attached
int index;
btVector3 m_splitv; // velocity associated with split impulse
btMatrix3x3 m_effectiveMass; // effective mass in contact
btMatrix3x3 m_effectiveMass_inv; // inverse of effective mass
};
/* Link */
ATTRIBUTE_ALIGNED16(struct)
@@ -352,6 +354,7 @@ public:
btScalar m_c2; // inverse mass of node/face
btScalar m_c3; // Friction
btScalar m_c4; // Hardness
btMatrix3x3 m_c5; // inverse effective mass
// jacobians and unit impulse responses for multibody
btMultiBodyJacobianData jacobianData_normal;

17
src/BulletSoftBody/btSoftBodyInternals.h
View File

@@ -1007,6 +1007,16 @@ static inline btMatrix3x3 ImpulseMatrix(btScalar dt,
return (Diagonal(1 / dt) * Add(Diagonal(ima), MassMatrix(imb, iwi, r)).inverse());
}
//
static inline btMatrix3x3 ImpulseMatrix(btScalar dt,
const btMatrix3x3& effective_mass,
btScalar imb,
const btMatrix3x3& iwi,
const btVector3& r)
{
return (Diagonal(1 / dt) * Add(effective_mass, MassMatrix(imb, iwi, r)).inverse());
}
//
static inline btMatrix3x3 ImpulseMatrix(btScalar ima, const btMatrix3x3& iia, const btVector3& ra,
btScalar imb, const btMatrix3x3& iib, const btVector3& rb)
@@ -1680,7 +1690,9 @@ struct btSoftColliders
const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
const btVector3 ra = n.m_x - wtr.getOrigin();
c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
c.m_c0 = ImpulseMatrix(1, n.m_effectiveMass_inv, imb, iwi, ra);
c.m_c5 = n.m_effectiveMass_inv;
// c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
c.m_c1 = ra;
}
else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
@@ -1708,7 +1720,7 @@ struct btSoftColliders
t1.getX(), t1.getY(), t1.getZ(),
t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame
const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
btMatrix3x3 local_impulse_matrix = (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
btMatrix3x3 local_impulse_matrix = (n.m_effectiveMass_inv + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
c.jacobianData_normal = jacobianData_normal;
c.jacobianData_t1 = jacobianData_t1;
@@ -1774,6 +1786,7 @@ struct btSoftColliders
c.m_c2 = ima;
c.m_c3 = fc;
c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
c.m_c5 = Diagonal(ima);
if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
{
const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();