diff options
Diffstat (limited to 'src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.cpp')
| -rw-r--r-- | src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.cpp | 325 |
1 files changed, 325 insertions, 0 deletions
diff --git a/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.cpp b/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.cpp new file mode 100644 index 000000000..1418cc247 --- /dev/null +++ b/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.cpp @@ -0,0 +1,325 @@ +#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; +}
\ No newline at end of file |