diff options
Diffstat (limited to 'src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp')
| -rw-r--r-- | src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp | 266 |
1 files changed, 266 insertions, 0 deletions
diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp new file mode 100644 index 000000000..3b3b62edc --- /dev/null +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -0,0 +1,266 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +/* ====== Overview of the Deformable Algorithm ====== */ + +/* +A single step of the deformable body simulation contains the following main components: +1. Update velocity to a temporary state v_{n+1}^* = v_n + explicit_force * dt / mass, where explicit forces include gravity and elastic forces. +2. Detect collisions between rigid and deformable bodies at position x_{n+1}^* = x_n + dt * v_{n+1}^*. +3. Then velocities of deformable bodies v_{n+1} are solved in + M(v_{n+1} - v_{n+1}^*) = damping_force * dt / mass, + by a conjugate gradient solver, where the damping force is implicit and depends on v_{n+1}. +4. Contact constraints are solved as projections as in the paper by Baraff and Witkin https://www.cs.cmu.edu/~baraff/papers/sig98.pdf. Dynamic frictions are treated as a force and added to the rhs of the CG solve, whereas static frictions are treated as constraints similar to contact. +5. Position is updated via x_{n+1} = x_n + dt * v_{n+1}. +6. Apply position correction to prevent numerical drift. + +The algorithm also closely resembles the one in http://physbam.stanford.edu/~fedkiw/papers/stanford2008-03.pdf + */ + +#include <stdio.h> +#include "btDeformableRigidDynamicsWorld.h" +#include "btDeformableBodySolver.h" +#include "LinearMath/btQuickprof.h" + +void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) +{ + BT_PROFILE("internalSingleStepSimulation"); + reinitialize(timeStep); + // add gravity to velocity of rigid and multi bodys + applyRigidBodyGravity(timeStep); + + ///apply gravity and explicit force to velocity, predict motion + predictUnconstraintMotion(timeStep); + + ///perform collision detection + btMultiBodyDynamicsWorld::performDiscreteCollisionDetection(); + + btMultiBodyDynamicsWorld::calculateSimulationIslands(); + + beforeSolverCallbacks(timeStep); + + ///solve deformable bodies constraints + solveDeformableBodiesConstraints(timeStep); + + afterSolverCallbacks(timeStep); + + integrateTransforms(timeStep); + + ///update vehicle simulation + btMultiBodyDynamicsWorld::updateActions(timeStep); + + btMultiBodyDynamicsWorld::updateActivationState(timeStep); + // End solver-wise simulation step + // /////////////////////////////// +} + +void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt) +{ + // perform position correction for all constraints + BT_PROFILE("positionCorrection"); + for (int index = 0; index < m_deformableBodySolver->m_objective->projection.m_constraints.size(); ++index) + { + btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_deformableBodySolver->m_objective->projection.m_frictions[m_deformableBodySolver->m_objective->projection.m_constraints.getKeyAtIndex(index)]; + btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_deformableBodySolver->m_objective->projection.m_constraints.getAtIndex(index); + for (int i = 0; i < constraints.size(); ++i) + { + DeformableContactConstraint& constraint = constraints[i]; + DeformableFrictionConstraint& friction = frictions[i]; + for (int j = 0; j < constraint.m_contact.size(); ++j) + { + const btSoftBody::RContact* c = constraint.m_contact[j]; + // skip anchor points + if (c == NULL || c->m_node->m_im == 0) + continue; + const btSoftBody::sCti& cti = c->m_cti; + btVector3 va(0, 0, 0); + + // grab the velocity of the rigid body + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + btRigidBody* rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)): btVector3(0, 0, 0); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0]; + const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0]; + const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0]; + const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector(); + // add in the normal component of the va + btScalar vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += local_v[k] * J_n[k]; + } + va = cti.m_normal * vel; + + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += local_v[k] * J_t1[k]; + } + va += c->t1 * vel; + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += local_v[k] * J_t2[k]; + } + va += c->t2 * vel; + } + } + else + { + // The object interacting with deformable node is not supported for position correction + btAssert(false); + } + + if (cti.m_colObj->hasContactResponse()) + { + btScalar dp = cti.m_offset; + + // only perform position correction when penetrating + if (dp < 0) + { + if (friction.m_static[j] == true) + { + c->m_node->m_v = va; + } + c->m_node->m_v -= dp * cti.m_normal / dt; + } + } + } + } + } +} + + +void btDeformableRigidDynamicsWorld::integrateTransforms(btScalar dt) +{ + BT_PROFILE("integrateTransforms"); + m_deformableBodySolver->backupVelocity(); + positionCorrection(dt); + btMultiBodyDynamicsWorld::integrateTransforms(dt); + 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]; + node.m_x = node.m_q + dt * node.m_v; + } + } + m_deformableBodySolver->revertVelocity(); +} + +void btDeformableRigidDynamicsWorld::solveDeformableBodiesConstraints(btScalar timeStep) +{ + m_deformableBodySolver->solveConstraints(timeStep); +} + +void btDeformableRigidDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask) +{ + m_softBodies.push_back(body); + + // Set the soft body solver that will deal with this body + // to be the world's solver + body->setSoftBodySolver(m_deformableBodySolver); + + btCollisionWorld::addCollisionObject(body, + collisionFilterGroup, + collisionFilterMask); +} + +void btDeformableRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) +{ + BT_PROFILE("predictUnconstraintMotion"); + btMultiBodyDynamicsWorld::predictUnconstraintMotion(timeStep); + m_deformableBodySolver->predictMotion(timeStep); +} + +void btDeformableRigidDynamicsWorld::reinitialize(btScalar timeStep) +{ + m_internalTime += timeStep; + m_deformableBodySolver->reinitialize(m_softBodies, timeStep); + btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo(); + dispatchInfo.m_timeStep = timeStep; + dispatchInfo.m_stepCount = 0; + dispatchInfo.m_debugDraw = btMultiBodyDynamicsWorld::getDebugDrawer(); + btMultiBodyDynamicsWorld::getSolverInfo().m_timeStep = timeStep; +} + +void btDeformableRigidDynamicsWorld::applyRigidBodyGravity(btScalar timeStep) +{ + // 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(); + // integrate rigid body gravity + for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i) + { + btRigidBody* rb = m_nonStaticRigidBodies[i]; + rb->integrateVelocities(timeStep); + } + // integrate multibody gravity + btMultiBodyDynamicsWorld::solveExternalForces(btMultiBodyDynamicsWorld::getSolverInfo()); + clearForces(); + clearMultiBodyForces(); +} + +void btDeformableRigidDynamicsWorld::beforeSolverCallbacks(btScalar timeStep) +{ + if (0 != m_internalTickCallback) + { + (*m_internalTickCallback)(this, timeStep); + } + + if (0 != m_solverCallback) + { + (*m_solverCallback)(m_internalTime, this); + } +} + +void btDeformableRigidDynamicsWorld::afterSolverCallbacks(btScalar timeStep) +{ + if (0 != m_solverCallback) + { + (*m_solverCallback)(m_internalTime, this); + } +} + +void btDeformableRigidDynamicsWorld::addForce(btSoftBody* psb, btDeformableLagrangianForce* force) +{ + btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf; + bool added = false; + for (int i = 0; i < forces.size(); ++i) + { + if (forces[i]->getForceType() == force->getForceType()) + { + forces[i]->addSoftBody(psb); + added = true; + break; + } + } + if (!added) + { + force->addSoftBody(psb); + force->setIndices(m_deformableBodySolver->m_objective->getIndices()); + forces.push_back(force); + } +} |