267 lines
11 KiB
C++
267 lines
11 KiB
C++
/*
|
|
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);
|
|
}
|
|
}
|