325 lines
9.7 KiB
C++
325 lines
9.7 KiB
C++
|
#include "btReducedDeformableBodySolver.h"
|
||
|
|
#include "btReducedDeformableBody.h"
|
||
|
|
|
||
|
|
btReducedDeformableBodySolver::btReducedDeformableBodySolver()
|
||
|
|
{
|
||
|
|
m_ascendOrder = true;
|
||
|
|
m_reducedSolver = true;
|
||
|
|
m_dampingAlpha = 0;
|
||
|
|
m_dampingBeta = 0;
|
||
|
|
m_gravity = btVector3(0, 0, 0);
|
||
|
|
}
|
||
|
|
|
||
|
|
void btReducedDeformableBodySolver::setGravity(const btVector3& gravity)
|
||
|
|
{
|
||
|
|
m_gravity = gravity;
|
||
|
|
}
|
||
|
|
|
||
|
|
void btReducedDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody*>& bodies, btScalar dt)
|
||
|
|
{
|
||
|
|
m_softBodies.copyFromArray(bodies);
|
||
|
|
bool nodeUpdated = updateNodes();
|
||
|
|
|
||
|
|
if (nodeUpdated)
|
||
|
|
{
|
||
|
|
m_dv.resize(m_numNodes, btVector3(0, 0, 0));
|
||
|
|
m_ddv.resize(m_numNodes, btVector3(0, 0, 0));
|
||
|
|
m_residual.resize(m_numNodes, btVector3(0, 0, 0));
|
||
|
|
m_backupVelocity.resize(m_numNodes, btVector3(0, 0, 0));
|
||
|
|
}
|
||
|
|
|
||
|
|
// need to setZero here as resize only set value for newly allocated items
|
||
|
|
for (int i = 0; i < m_numNodes; ++i)
|
||
|
|
{
|
||
|
|
m_dv[i].setZero();
|
||
|
|
m_ddv[i].setZero();
|
||
|
|
m_residual[i].setZero();
|
||
|
|
}
|
||
|
|
|
||
|
|
if (dt > 0)
|
||
|
|
{
|
||
|
|
m_dt = dt;
|
||
|
|
}
|
||
|
|
m_objective->reinitialize(nodeUpdated, dt);
|
||
|
|
|
||
|
|
int N = bodies.size();
|
||
|
|
if (nodeUpdated)
|
||
|
|
{
|
||
|
|
m_staticConstraints.resize(N);
|
||
|
|
m_nodeRigidConstraints.resize(N);
|
||
|
|
// m_faceRigidConstraints.resize(N);
|
||
|
|
}
|
||
|
|
for (int i = 0; i < N; ++i)
|
||
|
|
{
|
||
|
|
m_staticConstraints[i].clear();
|
||
|
|
m_nodeRigidConstraints[i].clear();
|
||
|
|
// m_faceRigidConstraints[i].clear();
|
||
|
|
}
|
||
|
|
|
||
|
|
for (int i = 0; i < m_softBodies.size(); ++i)
|
||
|
|
{
|
||
|
|
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
|
||
|
|
rsb->m_contactNodesList.clear();
|
||
|
|
}
|
||
|
|
|
||
|
|
// set node index offsets
|
||
|
|
int sum = 0;
|
||
|
|
for (int i = 0; i < m_softBodies.size(); ++i)
|
||
|
|
{
|
||
|
|
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
|
||
|
|
rsb->m_nodeIndexOffset = sum;
|
||
|
|
sum += rsb->m_nodes.size();
|
||
|
|
}
|
||
|
|
|
||
|
|
btDeformableBodySolver::updateSoftBodies();
|
||
|
|
}
|
||
|
|
|
||
|
|
void btReducedDeformableBodySolver::predictMotion(btScalar solverdt)
|
||
|
|
{
|
||
|
|
applyExplicitForce(solverdt);
|
||
|
|
|
||
|
|
// predict new mesh location
|
||
|
|
predictReduceDeformableMotion(solverdt);
|
||
|
|
|
||
|
|
//TODO: check if there is anything missed from btDeformableBodySolver::predictDeformableMotion
|
||
|
|
}
|
||
|
|
|
||
|
|
void btReducedDeformableBodySolver::predictReduceDeformableMotion(btScalar solverdt)
|
||
|
|
{
|
||
|
|
for (int i = 0; i < m_softBodies.size(); ++i)
|
||
|
|
{
|
||
|
|
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
|
||
|
|
if (!rsb->isActive())
|
||
|
|
{
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
|
||
|
|
// clear contacts variables
|
||
|
|
rsb->m_nodeRigidContacts.resize(0);
|
||
|
|
rsb->m_faceRigidContacts.resize(0);
|
||
|
|
rsb->m_faceNodeContacts.resize(0);
|
||
|
|
|
||
|
|
// calculate inverse mass matrix for all nodes
|
||
|
|
for (int j = 0; j < rsb->m_nodes.size(); ++j)
|
||
|
|
{
|
||
|
|
if (rsb->m_nodes[j].m_im > 0)
|
||
|
|
{
|
||
|
|
rsb->m_nodes[j].m_effectiveMass_inv = rsb->m_nodes[j].m_effectiveMass.inverse();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// rigid motion: t, R at time^*
|
||
|
|
rsb->predictIntegratedTransform(solverdt, rsb->getInterpolationWorldTransform());
|
||
|
|
|
||
|
|
// update reduced dofs at time^*
|
||
|
|
// rsb->updateReducedDofs(solverdt);
|
||
|
|
|
||
|
|
// update local moment arm at time^*
|
||
|
|
// rsb->updateLocalMomentArm();
|
||
|
|
// rsb->updateExternalForceProjectMatrix(true);
|
||
|
|
|
||
|
|
// predict full space velocity at time^* (needed for constraints)
|
||
|
|
rsb->mapToFullVelocity(rsb->getInterpolationWorldTransform());
|
||
|
|
|
||
|
|
// update full space nodal position at time^*
|
||
|
|
rsb->mapToFullPosition(rsb->getInterpolationWorldTransform());
|
||
|
|
|
||
|
|
// update bounding box
|
||
|
|
rsb->updateBounds();
|
||
|
|
|
||
|
|
// update tree
|
||
|
|
rsb->updateNodeTree(true, true);
|
||
|
|
if (!rsb->m_fdbvt.empty())
|
||
|
|
{
|
||
|
|
rsb->updateFaceTree(true, true);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void btReducedDeformableBodySolver::applyExplicitForce(btScalar solverdt)
|
||
|
|
{
|
||
|
|
for (int i = 0; i < m_softBodies.size(); ++i)
|
||
|
|
{
|
||
|
|
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
|
||
|
|
|
||
|
|
// apply gravity to the rigid frame, get m_linearVelocity at time^*
|
||
|
|
rsb->applyRigidGravity(m_gravity, solverdt);
|
||
|
|
|
||
|
|
if (!rsb->isReducedModesOFF())
|
||
|
|
{
|
||
|
|
// add internal force (elastic force & damping force)
|
||
|
|
rsb->applyReducedElasticForce(rsb->m_reducedDofsBuffer);
|
||
|
|
rsb->applyReducedDampingForce(rsb->m_reducedVelocityBuffer);
|
||
|
|
|
||
|
|
// get reduced velocity at time^*
|
||
|
|
rsb->updateReducedVelocity(solverdt);
|
||
|
|
}
|
||
|
|
|
||
|
|
// apply damping (no need at this point)
|
||
|
|
// rsb->applyDamping(solverdt);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void btReducedDeformableBodySolver::applyTransforms(btScalar timeStep)
|
||
|
|
{
|
||
|
|
for (int i = 0; i < m_softBodies.size(); ++i)
|
||
|
|
{
|
||
|
|
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
|
||
|
|
|
||
|
|
// rigid motion
|
||
|
|
rsb->proceedToTransform(timeStep, true);
|
||
|
|
|
||
|
|
if (!rsb->isReducedModesOFF())
|
||
|
|
{
|
||
|
|
// update reduced dofs for time^n+1
|
||
|
|
rsb->updateReducedDofs(timeStep);
|
||
|
|
|
||
|
|
// update local moment arm for time^n+1
|
||
|
|
rsb->updateLocalMomentArm();
|
||
|
|
rsb->updateExternalForceProjectMatrix(true);
|
||
|
|
}
|
||
|
|
|
||
|
|
// update mesh nodal positions for time^n+1
|
||
|
|
rsb->mapToFullPosition(rsb->getRigidTransform());
|
||
|
|
|
||
|
|
// update mesh nodal velocity
|
||
|
|
rsb->mapToFullVelocity(rsb->getRigidTransform());
|
||
|
|
|
||
|
|
// end of time step clean up and update
|
||
|
|
rsb->endOfTimeStepZeroing();
|
||
|
|
|
||
|
|
// update the rendering mesh
|
||
|
|
rsb->interpolateRenderMesh();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void btReducedDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal)
|
||
|
|
{
|
||
|
|
for (int i = 0; i < m_softBodies.size(); ++i)
|
||
|
|
{
|
||
|
|
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
|
||
|
|
if (!rsb->isActive())
|
||
|
|
{
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
|
||
|
|
// set fixed constraints
|
||
|
|
for (int j = 0; j < rsb->m_fixedNodes.size(); ++j)
|
||
|
|
{
|
||
|
|
int i_node = rsb->m_fixedNodes[j];
|
||
|
|
if (rsb->m_nodes[i_node].m_im == 0)
|
||
|
|
{
|
||
|
|
for (int k = 0; k < 3; ++k)
|
||
|
|
{
|
||
|
|
btVector3 dir(0, 0, 0);
|
||
|
|
dir[k] = 1;
|
||
|
|
btReducedDeformableStaticConstraint static_constraint(rsb, &rsb->m_nodes[i_node], rsb->getRelativePos(i_node), rsb->m_x0[i_node], dir, infoGlobal, m_dt);
|
||
|
|
m_staticConstraints[i].push_back(static_constraint);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
btAssert(rsb->m_fixedNodes.size() * 3 == m_staticConstraints[i].size());
|
||
|
|
|
||
|
|
// set Deformable Node vs. Rigid constraint
|
||
|
|
for (int j = 0; j < rsb->m_nodeRigidContacts.size(); ++j)
|
||
|
|
{
|
||
|
|
const btSoftBody::DeformableNodeRigidContact& contact = rsb->m_nodeRigidContacts[j];
|
||
|
|
// skip fixed points
|
||
|
|
if (contact.m_node->m_im == 0)
|
||
|
|
{
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
btReducedDeformableNodeRigidContactConstraint constraint(rsb, contact, infoGlobal, m_dt);
|
||
|
|
m_nodeRigidConstraints[i].push_back(constraint);
|
||
|
|
rsb->m_contactNodesList.push_back(contact.m_node->index - rsb->m_nodeIndexOffset);
|
||
|
|
}
|
||
|
|
// std::cout << "contact node list size: " << rsb->m_contactNodesList.size() << "\n";
|
||
|
|
// std::cout << "#contact nodes: " << m_nodeRigidConstraints[i].size() << "\n";
|
||
|
|
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
btScalar btReducedDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
|
||
|
|
{
|
||
|
|
btScalar residualSquare = 0;
|
||
|
|
|
||
|
|
for (int i = 0; i < m_softBodies.size(); ++i)
|
||
|
|
{
|
||
|
|
btAlignedObjectArray<int> m_orderNonContactConstraintPool;
|
||
|
|
btAlignedObjectArray<int> m_orderContactConstraintPool;
|
||
|
|
|
||
|
|
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
|
||
|
|
|
||
|
|
// shuffle the order of applying constraint
|
||
|
|
m_orderNonContactConstraintPool.resize(m_staticConstraints[i].size());
|
||
|
|
m_orderContactConstraintPool.resize(m_nodeRigidConstraints[i].size());
|
||
|
|
if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
|
||
|
|
{
|
||
|
|
// fixed constraint order
|
||
|
|
for (int j = 0; j < m_staticConstraints[i].size(); ++j)
|
||
|
|
{
|
||
|
|
m_orderNonContactConstraintPool[j] = m_ascendOrder ? j : m_staticConstraints[i].size() - 1 - j;
|
||
|
|
}
|
||
|
|
// contact constraint order
|
||
|
|
for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
|
||
|
|
{
|
||
|
|
m_orderContactConstraintPool[j] = m_ascendOrder ? j : m_nodeRigidConstraints[i].size() - 1 - j;
|
||
|
|
}
|
||
|
|
|
||
|
|
m_ascendOrder = m_ascendOrder ? false : true;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
for (int j = 0; j < m_staticConstraints[i].size(); ++j)
|
||
|
|
{
|
||
|
|
m_orderNonContactConstraintPool[j] = j;
|
||
|
|
}
|
||
|
|
// contact constraint order
|
||
|
|
for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
|
||
|
|
{
|
||
|
|
m_orderContactConstraintPool[j] = j;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// handle fixed constraint
|
||
|
|
for (int k = 0; k < m_staticConstraints[i].size(); ++k)
|
||
|
|
{
|
||
|
|
btReducedDeformableStaticConstraint& constraint = m_staticConstraints[i][m_orderNonContactConstraintPool[k]];
|
||
|
|
btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
|
||
|
|
residualSquare = btMax(residualSquare, localResidualSquare);
|
||
|
|
}
|
||
|
|
|
||
|
|
// handle contact constraint
|
||
|
|
|
||
|
|
// node vs rigid contact
|
||
|
|
// std::cout << "!!#contact_nodes: " << m_nodeRigidConstraints[i].size() << '\n';
|
||
|
|
for (int k = 0; k < m_nodeRigidConstraints[i].size(); ++k)
|
||
|
|
{
|
||
|
|
btReducedDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][m_orderContactConstraintPool[k]];
|
||
|
|
btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
|
||
|
|
residualSquare = btMax(residualSquare, localResidualSquare);
|
||
|
|
}
|
||
|
|
|
||
|
|
// face vs rigid contact
|
||
|
|
// for (int k = 0; k < m_faceRigidConstraints[i].size(); ++k)
|
||
|
|
// {
|
||
|
|
// btReducedDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][k];
|
||
|
|
// btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
|
||
|
|
// residualSquare = btMax(residualSquare, localResidualSquare);
|
||
|
|
// }
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
return residualSquare;
|
||
|
|
}
|
||
|
|
|
||
|
|
void btReducedDeformableBodySolver::deformableBodyInternalWriteBack()
|
||
|
|
{
|
||
|
|
// reduced deformable update
|
||
|
|
for (int i = 0; i < m_softBodies.size(); ++i)
|
||
|
|
{
|
||
|
|
btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
|
||
|
|
rsb->applyInternalVelocityChanges();
|
||
|
|
}
|
||
|
|
m_ascendOrder = true;
|
||
|
|
}
|