Scarfski/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3.git synced 2026-10-09 18:35:33 +00:00
Code Issues Packages Projects Releases Wiki Activity

Revert "Revert "Reduced Deformable Model""

Browse Source
This commit is contained in:
erwincoumans
2022-03-07 16:35:07 -08:00
committed by GitHub
parent aee1ab63fe
commit 4fbecfeddc
77 changed files with 30447 additions and 114 deletions
Download Patch File Download Diff File Expand all files Collapse all files

89
src/BulletSoftBody/btDeformableBodySolver.cpp
View File

@@ -23,6 +23,7 @@ btDeformableBodySolver::btDeformableBodySolver()
: m_numNodes(0), m_cg(kMaxConjugateGradientIterations), m_cr(kMaxConjugateGradientIterations), m_maxNewtonIterations(1), m_newtonTolerance(1e-4), m_lineSearch(false), m_useProjection(false)
{
m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity);
m_reducedSolver = false;
}
btDeformableBodySolver::~btDeformableBodySolver()
@@ -401,7 +402,7 @@ void btDeformableBodySolver::predictMotion(btScalar solverdt)
}
}
}
m_objective->applyExplicitForce(m_residual);
applyExplicitForce();
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
@@ -504,3 +505,89 @@ void btDeformableBodySolver::setLineSearch(bool lineSearch)
{
m_lineSearch = lineSearch;
}
void btDeformableBodySolver::applyExplicitForce()
{
m_objective->applyExplicitForce(m_residual);
}
void btDeformableBodySolver::applyTransforms(btScalar timeStep)
{
for (int i = 0; i < m_softBodies.size(); ++i)
{
btSoftBody* psb = m_softBodies[i];
for (int j = 0; j < psb->m_nodes.size(); ++j)
{
btSoftBody::Node& node = psb->m_nodes[j];
btScalar maxDisplacement = psb->getWorldInfo()->m_maxDisplacement;
btScalar clampDeltaV = maxDisplacement / timeStep;
for (int c = 0; c < 3; c++)
{
if (node.m_v[c] > clampDeltaV)
{
node.m_v[c] = clampDeltaV;
}
if (node.m_v[c] < -clampDeltaV)
{
node.m_v[c] = -clampDeltaV;
}
}
node.m_x = node.m_x + timeStep * (node.m_v + node.m_splitv);
node.m_q = node.m_x;
node.m_vn = node.m_v;
}
// enforce anchor constraints
for (int j = 0; j < psb->m_deformableAnchors.size(); ++j)
{
btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j];
btSoftBody::Node* n = a.m_node;
n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local;
// update multibody anchor info
if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
{
btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(a.m_cti.m_colObj);
if (multibodyLinkCol)
{
btVector3 nrm;
const btCollisionShape* shp = multibodyLinkCol->getCollisionShape();
const btTransform& wtr = multibodyLinkCol->getWorldTransform();
psb->m_worldInfo->m_sparsesdf.Evaluate(
wtr.invXform(n->m_x),
shp,
nrm,
0);
a.m_cti.m_normal = wtr.getBasis() * nrm;
btVector3 normal = a.m_cti.m_normal;
btVector3 t1 = generateUnitOrthogonalVector(normal);
btVector3 t2 = btCross(normal, t1);
btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal);
findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1);
findJacobian(multibodyLinkCol, jacobianData_t2, a.m_node->m_x, t2);
btScalar* J_n = &jacobianData_normal.m_jacobians[0];
btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
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();
a.m_c0 = rot.transpose() * local_impulse_matrix * rot;
a.jacobianData_normal = jacobianData_normal;
a.jacobianData_t1 = jacobianData_t1;
a.jacobianData_t2 = jacobianData_t2;
a.t1 = t1;
a.t2 = t2;
}
}
}
psb->interpolateRenderMesh();
}
}