2020-01-08 17:05:43 +00:00
/*
Written by Xuchen Han < xuchenhan2015 @ u . northwestern . edu >
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 :
Call internalStepSimulation multiple times , to achieve 240 Hz ( 4 steps of 60 Hz ) .
1. Deformable maintaintenance of rest lengths and volume preservation . Forces only depend on position : 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 discrete collisions between rigid and deformable bodies at position x_ { n + 1 } ^ * = x_n + dt * v_ { n + 1 } ^ * .
3 a . Solve all constraints , including LCP . Contact , position correction due to numerical drift , friction , and anchors for deformable .
3 b . 5 Newton steps ( multiple step ) . Conjugent Gradient solves linear system . Deformable Damping : 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 } .
Make sure contact constraints are not violated in step b by performing velocity 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.
4. Position is updated via x_ { n + 1 } = x_n + dt * v_ { n + 1 } .
The algorithm also closely resembles the one in http : //physbam.stanford.edu/~fedkiw/papers/stanford2008-03.pdf
*/
# include <stdio.h>
# include "btDeformableMultiBodyDynamicsWorld.h"
# include "DeformableBodyInplaceSolverIslandCallback.h"
# include "btDeformableBodySolver.h"
# include "LinearMath/btQuickprof.h"
# include "btSoftBodyInternals.h"
btDeformableMultiBodyDynamicsWorld : : btDeformableMultiBodyDynamicsWorld ( btDispatcher * dispatcher , btBroadphaseInterface * pairCache , btDeformableMultiBodyConstraintSolver * constraintSolver , btCollisionConfiguration * collisionConfiguration , btDeformableBodySolver * deformableBodySolver )
2021-09-29 13:47:08 +00:00
: btMultiBodyDynamicsWorld ( dispatcher , pairCache , ( btMultiBodyConstraintSolver * ) constraintSolver , collisionConfiguration ) ,
m_deformableBodySolver ( deformableBodySolver ) ,
m_solverCallback ( 0 )
2020-01-08 17:05:43 +00:00
{
m_drawFlags = fDrawFlags : : Std ;
m_drawNodeTree = true ;
m_drawFaceTree = false ;
m_drawClusterTree = false ;
m_sbi . m_broadphase = pairCache ;
m_sbi . m_dispatcher = dispatcher ;
m_sbi . m_sparsesdf . Initialize ( ) ;
m_sbi . m_sparsesdf . setDefaultVoxelsz ( 0.005 ) ;
m_sbi . m_sparsesdf . Reset ( ) ;
2021-09-29 13:47:08 +00:00
2020-01-08 17:05:43 +00:00
m_sbi . air_density = ( btScalar ) 1.2 ;
m_sbi . water_density = 0 ;
m_sbi . water_offset = 0 ;
m_sbi . water_normal = btVector3 ( 0 , 0 , 0 ) ;
2021-09-29 13:47:08 +00:00
m_sbi . m_gravity . setValue ( 0 , - 9.8 , 0 ) ;
2020-01-08 17:05:43 +00:00
m_internalTime = 0.0 ;
m_implicit = false ;
m_lineSearch = false ;
2021-09-29 13:47:08 +00:00
m_useProjection = false ;
m_ccdIterations = 5 ;
2020-01-08 17:05:43 +00:00
m_solverDeformableBodyIslandCallback = new DeformableBodyInplaceSolverIslandCallback ( constraintSolver , dispatcher ) ;
}
2021-09-29 13:47:08 +00:00
btDeformableMultiBodyDynamicsWorld : : ~ btDeformableMultiBodyDynamicsWorld ( )
{
delete m_solverDeformableBodyIslandCallback ;
}
2020-01-08 17:05:43 +00:00
void btDeformableMultiBodyDynamicsWorld : : internalSingleStepSimulation ( btScalar timeStep )
{
2021-09-29 13:47:08 +00:00
BT_PROFILE ( " internalSingleStepSimulation " ) ;
if ( 0 ! = m_internalPreTickCallback )
{
( * m_internalPreTickCallback ) ( this , timeStep ) ;
}
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 that involves rigid/multi bodies
btMultiBodyDynamicsWorld : : performDiscreteCollisionDetection ( ) ;
btMultiBodyDynamicsWorld : : calculateSimulationIslands ( ) ;
beforeSolverCallbacks ( timeStep ) ;
///solve contact constraints and then deformable bodies momemtum equation
solveConstraints ( timeStep ) ;
afterSolverCallbacks ( timeStep ) ;
performDeformableCollisionDetection ( ) ;
applyRepulsionForce ( timeStep ) ;
performGeometricCollisions ( timeStep ) ;
integrateTransforms ( timeStep ) ;
///update vehicle simulation
btMultiBodyDynamicsWorld : : updateActions ( timeStep ) ;
updateActivationState ( timeStep ) ;
// End solver-wise simulation step
// ///////////////////////////////
}
void btDeformableMultiBodyDynamicsWorld : : performDeformableCollisionDetection ( )
{
for ( int i = 0 ; i < m_softBodies . size ( ) ; + + i )
{
m_softBodies [ i ] - > m_softSoftCollision = true ;
}
for ( int i = 0 ; i < m_softBodies . size ( ) ; + + i )
{
for ( int j = i ; j < m_softBodies . size ( ) ; + + j )
{
m_softBodies [ i ] - > defaultCollisionHandler ( m_softBodies [ j ] ) ;
}
}
for ( int i = 0 ; i < m_softBodies . size ( ) ; + + i )
{
m_softBodies [ i ] - > m_softSoftCollision = false ;
}
2020-01-08 17:05:43 +00:00
}
void btDeformableMultiBodyDynamicsWorld : : updateActivationState ( btScalar timeStep )
{
2021-09-29 13:47:08 +00:00
for ( int i = 0 ; i < m_softBodies . size ( ) ; i + + )
{
btSoftBody * psb = m_softBodies [ i ] ;
psb - > updateDeactivation ( timeStep ) ;
if ( psb - > wantsSleeping ( ) )
{
if ( psb - > getActivationState ( ) = = ACTIVE_TAG )
psb - > setActivationState ( WANTS_DEACTIVATION ) ;
if ( psb - > getActivationState ( ) = = ISLAND_SLEEPING )
{
psb - > setZeroVelocity ( ) ;
}
}
else
{
if ( psb - > getActivationState ( ) ! = DISABLE_DEACTIVATION )
psb - > setActivationState ( ACTIVE_TAG ) ;
}
}
btMultiBodyDynamicsWorld : : updateActivationState ( timeStep ) ;
2020-01-08 17:05:43 +00:00
}
2021-09-29 13:47:08 +00:00
void btDeformableMultiBodyDynamicsWorld : : applyRepulsionForce ( btScalar timeStep )
{
BT_PROFILE ( " btDeformableMultiBodyDynamicsWorld::applyRepulsionForce " ) ;
for ( int i = 0 ; i < m_softBodies . size ( ) ; i + + )
{
btSoftBody * psb = m_softBodies [ i ] ;
if ( psb - > isActive ( ) )
{
psb - > applyRepulsionForce ( timeStep , true ) ;
}
}
}
void btDeformableMultiBodyDynamicsWorld : : performGeometricCollisions ( btScalar timeStep )
{
BT_PROFILE ( " btDeformableMultiBodyDynamicsWorld::performGeometricCollisions " ) ;
// refit the BVH tree for CCD
for ( int i = 0 ; i < m_softBodies . size ( ) ; + + i )
{
btSoftBody * psb = m_softBodies [ i ] ;
if ( psb - > isActive ( ) )
{
m_softBodies [ i ] - > updateFaceTree ( true , false ) ;
m_softBodies [ i ] - > updateNodeTree ( true , false ) ;
for ( int j = 0 ; j < m_softBodies [ i ] - > m_faces . size ( ) ; + + j )
{
btSoftBody : : Face & f = m_softBodies [ i ] - > m_faces [ j ] ;
f . m_n0 = ( f . m_n [ 1 ] - > m_x - f . m_n [ 0 ] - > m_x ) . cross ( f . m_n [ 2 ] - > m_x - f . m_n [ 0 ] - > m_x ) ;
}
}
}
// clear contact points & update DBVT
for ( int r = 0 ; r < m_ccdIterations ; + + r )
{
for ( int i = 0 ; i < m_softBodies . size ( ) ; + + i )
{
btSoftBody * psb = m_softBodies [ i ] ;
if ( psb - > isActive ( ) )
{
// clear contact points in the previous iteration
psb - > m_faceNodeContacts . clear ( ) ;
// update m_q and normals for CCD calculation
for ( int j = 0 ; j < psb - > m_nodes . size ( ) ; + + j )
{
psb - > m_nodes [ j ] . m_q = psb - > m_nodes [ j ] . m_x + timeStep * psb - > m_nodes [ j ] . m_v ;
}
for ( int j = 0 ; j < psb - > m_faces . size ( ) ; + + j )
{
btSoftBody : : Face & f = psb - > m_faces [ j ] ;
f . m_n1 = ( f . m_n [ 1 ] - > m_q - f . m_n [ 0 ] - > m_q ) . cross ( f . m_n [ 2 ] - > m_q - f . m_n [ 0 ] - > m_q ) ;
f . m_vn = ( f . m_n [ 1 ] - > m_v - f . m_n [ 0 ] - > m_v ) . cross ( f . m_n [ 2 ] - > m_v - f . m_n [ 0 ] - > m_v ) * timeStep * timeStep ;
}
}
}
// apply CCD to register new contact points
for ( int i = 0 ; i < m_softBodies . size ( ) ; + + i )
{
for ( int j = i ; j < m_softBodies . size ( ) ; + + j )
{
btSoftBody * psb1 = m_softBodies [ i ] ;
btSoftBody * psb2 = m_softBodies [ j ] ;
if ( psb1 - > isActive ( ) & & psb2 - > isActive ( ) )
{
m_softBodies [ i ] - > geometricCollisionHandler ( m_softBodies [ j ] ) ;
}
}
}
int penetration_count = 0 ;
for ( int i = 0 ; i < m_softBodies . size ( ) ; + + i )
{
btSoftBody * psb = m_softBodies [ i ] ;
if ( psb - > isActive ( ) )
{
penetration_count + = psb - > m_faceNodeContacts . size ( ) ;
}
}
if ( penetration_count = = 0 )
{
break ;
}
// apply inelastic impulse
for ( int i = 0 ; i < m_softBodies . size ( ) ; + + i )
{
btSoftBody * psb = m_softBodies [ i ] ;
if ( psb - > isActive ( ) )
{
psb - > applyRepulsionForce ( timeStep , false ) ;
}
}
}
}
2020-01-08 17:05:43 +00:00
void btDeformableMultiBodyDynamicsWorld : : softBodySelfCollision ( )
{
2021-09-29 13:47:08 +00:00
BT_PROFILE ( " btDeformableMultiBodyDynamicsWorld::softBodySelfCollision " ) ;
for ( int i = 0 ; i < m_softBodies . size ( ) ; i + + )
{
btSoftBody * psb = m_softBodies [ i ] ;
if ( psb - > isActive ( ) )
{
psb - > defaultCollisionHandler ( psb ) ;
}
}
2020-01-08 17:05:43 +00:00
}
void btDeformableMultiBodyDynamicsWorld : : positionCorrection ( btScalar timeStep )
{
2021-09-29 13:47:08 +00:00
// correct the position of rigid bodies with temporary velocity generated from split impulse
btContactSolverInfo infoGlobal ;
btVector3 zero ( 0 , 0 , 0 ) ;
for ( int i = 0 ; i < m_nonStaticRigidBodies . size ( ) ; + + i )
{
btRigidBody * rb = m_nonStaticRigidBodies [ i ] ;
//correct the position/orientation based on push/turn recovery
btTransform newTransform ;
btVector3 pushVelocity = rb - > getPushVelocity ( ) ;
btVector3 turnVelocity = rb - > getTurnVelocity ( ) ;
if ( pushVelocity [ 0 ] ! = 0.f | | pushVelocity [ 1 ] ! = 0 | | pushVelocity [ 2 ] ! = 0 | | turnVelocity [ 0 ] ! = 0.f | | turnVelocity [ 1 ] ! = 0 | | turnVelocity [ 2 ] ! = 0 )
{
btTransformUtil : : integrateTransform ( rb - > getWorldTransform ( ) , pushVelocity , turnVelocity * infoGlobal . m_splitImpulseTurnErp , timeStep , newTransform ) ;
rb - > setWorldTransform ( newTransform ) ;
rb - > setPushVelocity ( zero ) ;
rb - > setTurnVelocity ( zero ) ;
}
}
2020-01-08 17:05:43 +00:00
}
void btDeformableMultiBodyDynamicsWorld : : integrateTransforms ( btScalar timeStep )
{
2021-09-29 13:47:08 +00:00
BT_PROFILE ( " integrateTransforms " ) ;
positionCorrection ( timeStep ) ;
btMultiBodyDynamicsWorld : : integrateTransforms ( 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 ( ) ;
}
2020-01-08 17:05:43 +00:00
}
void btDeformableMultiBodyDynamicsWorld : : solveConstraints ( btScalar timeStep )
{
2021-09-29 13:47:08 +00:00
BT_PROFILE ( " btDeformableMultiBodyDynamicsWorld::solveConstraints " ) ;
// save v_{n+1}^* velocity after explicit forces
m_deformableBodySolver - > backupVelocity ( ) ;
// set up constraints among multibodies and between multibodies and deformable bodies
setupConstraints ( ) ;
// solve contact constraints
solveContactConstraints ( ) ;
// set up the directions in which the velocity does not change in the momentum solve
if ( m_useProjection )
m_deformableBodySolver - > m_objective - > m_projection . setProjection ( ) ;
else
m_deformableBodySolver - > m_objective - > m_projection . setLagrangeMultiplier ( ) ;
// for explicit scheme, m_backupVelocity = v_{n+1}^*
// for implicit scheme, m_backupVelocity = v_n
// Here, set dv = v_{n+1} - v_n for nodes in contact
m_deformableBodySolver - > setupDeformableSolve ( m_implicit ) ;
// At this point, dv should be golden for nodes in contact
// proceed to solve deformable momentum equation
m_deformableBodySolver - > solveDeformableConstraints ( timeStep ) ;
2020-01-08 17:05:43 +00:00
}
void btDeformableMultiBodyDynamicsWorld : : setupConstraints ( )
{
2021-09-29 13:47:08 +00:00
// set up constraints between multibody and deformable bodies
m_deformableBodySolver - > setConstraints ( m_solverInfo ) ;
// set up constraints among multibodies
{
sortConstraints ( ) ;
// setup the solver callback
btMultiBodyConstraint * * sortedMultiBodyConstraints = m_sortedMultiBodyConstraints . size ( ) ? & m_sortedMultiBodyConstraints [ 0 ] : 0 ;
btTypedConstraint * * constraintsPtr = getNumConstraints ( ) ? & m_sortedConstraints [ 0 ] : 0 ;
m_solverDeformableBodyIslandCallback - > setup ( & m_solverInfo , constraintsPtr , m_sortedConstraints . size ( ) , sortedMultiBodyConstraints , m_sortedMultiBodyConstraints . size ( ) , getDebugDrawer ( ) ) ;
// build islands
m_islandManager - > buildIslands ( getCollisionWorld ( ) - > getDispatcher ( ) , getCollisionWorld ( ) ) ;
}
2020-01-08 17:05:43 +00:00
}
void btDeformableMultiBodyDynamicsWorld : : sortConstraints ( )
{
2021-09-29 13:47:08 +00:00
m_sortedConstraints . resize ( m_constraints . size ( ) ) ;
int i ;
for ( i = 0 ; i < getNumConstraints ( ) ; i + + )
{
m_sortedConstraints [ i ] = m_constraints [ i ] ;
}
m_sortedConstraints . quickSort ( btSortConstraintOnIslandPredicate2 ( ) ) ;
m_sortedMultiBodyConstraints . resize ( m_multiBodyConstraints . size ( ) ) ;
for ( i = 0 ; i < m_multiBodyConstraints . size ( ) ; i + + )
{
m_sortedMultiBodyConstraints [ i ] = m_multiBodyConstraints [ i ] ;
}
m_sortedMultiBodyConstraints . quickSort ( btSortMultiBodyConstraintOnIslandPredicate ( ) ) ;
2020-01-08 17:05:43 +00:00
}
2021-09-29 13:47:08 +00:00
2020-01-08 17:05:43 +00:00
void btDeformableMultiBodyDynamicsWorld : : solveContactConstraints ( )
{
2021-09-29 13:47:08 +00:00
// process constraints on each island
m_islandManager - > processIslands ( getCollisionWorld ( ) - > getDispatcher ( ) , getCollisionWorld ( ) , m_solverDeformableBodyIslandCallback ) ;
// process deferred
m_solverDeformableBodyIslandCallback - > processConstraints ( ) ;
m_constraintSolver - > allSolved ( m_solverInfo , m_debugDrawer ) ;
// write joint feedback
{
for ( int i = 0 ; i < this - > m_multiBodies . size ( ) ; i + + )
{
btMultiBody * bod = m_multiBodies [ i ] ;
bool isSleeping = false ;
if ( bod - > getBaseCollider ( ) & & bod - > getBaseCollider ( ) - > getActivationState ( ) = = ISLAND_SLEEPING )
{
isSleeping = true ;
}
for ( int b = 0 ; b < bod - > getNumLinks ( ) ; b + + )
{
if ( bod - > getLink ( b ) . m_collider & & bod - > getLink ( b ) . m_collider - > getActivationState ( ) = = ISLAND_SLEEPING )
isSleeping = true ;
}
if ( ! isSleeping )
{
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
m_scratch_r . resize ( bod - > getNumLinks ( ) + 1 ) ; //multidof? ("Y"s use it and it is used to store qdd)
m_scratch_v . resize ( bod - > getNumLinks ( ) + 1 ) ;
m_scratch_m . resize ( bod - > getNumLinks ( ) + 1 ) ;
if ( bod - > internalNeedsJointFeedback ( ) )
{
if ( ! bod - > isUsingRK4Integration ( ) )
{
if ( bod - > internalNeedsJointFeedback ( ) )
{
bool isConstraintPass = true ;
bod - > computeAccelerationsArticulatedBodyAlgorithmMultiDof ( m_solverInfo . m_timeStep , m_scratch_r , m_scratch_v , m_scratch_m , isConstraintPass ,
getSolverInfo ( ) . m_jointFeedbackInWorldSpace ,
getSolverInfo ( ) . m_jointFeedbackInJointFrame ) ;
}
}
}
}
}
}
for ( int i = 0 ; i < this - > m_multiBodies . size ( ) ; i + + )
{
btMultiBody * bod = m_multiBodies [ i ] ;
bod - > processDeltaVeeMultiDof2 ( ) ;
}
2020-01-08 17:05:43 +00:00
}
void btDeformableMultiBodyDynamicsWorld : : addSoftBody ( btSoftBody * body , int collisionFilterGroup , int collisionFilterMask )
{
2021-09-29 13:47:08 +00:00
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 ) ;
2020-01-08 17:05:43 +00:00
}
void btDeformableMultiBodyDynamicsWorld : : predictUnconstraintMotion ( btScalar timeStep )
{
2021-09-29 13:47:08 +00:00
BT_PROFILE ( " predictUnconstraintMotion " ) ;
btMultiBodyDynamicsWorld : : predictUnconstraintMotion ( timeStep ) ;
m_deformableBodySolver - > predictMotion ( timeStep ) ;
2020-01-08 17:05:43 +00:00
}
void btDeformableMultiBodyDynamicsWorld : : reinitialize ( btScalar timeStep )
{
2021-09-29 13:47:08 +00:00
m_internalTime + = timeStep ;
m_deformableBodySolver - > setImplicit ( m_implicit ) ;
m_deformableBodySolver - > setLineSearch ( m_lineSearch ) ;
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 ;
if ( m_useProjection )
{
m_deformableBodySolver - > m_useProjection = true ;
m_deformableBodySolver - > m_objective - > m_projection . m_useStrainLimiting = true ;
m_deformableBodySolver - > m_objective - > m_preconditioner = m_deformableBodySolver - > m_objective - > m_massPreconditioner ;
}
else
{
m_deformableBodySolver - > m_useProjection = false ;
m_deformableBodySolver - > m_objective - > m_projection . m_useStrainLimiting = false ;
m_deformableBodySolver - > m_objective - > m_preconditioner = m_deformableBodySolver - > m_objective - > m_KKTPreconditioner ;
}
2020-01-08 17:05:43 +00:00
}
void btDeformableMultiBodyDynamicsWorld : : debugDrawWorld ( )
{
btMultiBodyDynamicsWorld : : debugDrawWorld ( ) ;
for ( int i = 0 ; i < getSoftBodyArray ( ) . size ( ) ; i + + )
{
btSoftBody * psb = ( btSoftBody * ) getSoftBodyArray ( ) [ i ] ;
{
btSoftBodyHelpers : : DrawFrame ( psb , getDebugDrawer ( ) ) ;
btSoftBodyHelpers : : Draw ( psb , getDebugDrawer ( ) , getDrawFlags ( ) ) ;
}
}
}
void btDeformableMultiBodyDynamicsWorld : : applyRigidBodyGravity ( btScalar timeStep )
{
2021-09-29 13:47:08 +00:00
// 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
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
{
forwardKinematics ( ) ;
clearMultiBodyConstraintForces ( ) ;
{
for ( int i = 0 ; i < this - > m_multiBodies . size ( ) ; i + + )
{
btMultiBody * bod = m_multiBodies [ i ] ;
bool isSleeping = false ;
if ( bod - > getBaseCollider ( ) & & bod - > getBaseCollider ( ) - > getActivationState ( ) = = ISLAND_SLEEPING )
{
isSleeping = true ;
}
for ( int b = 0 ; b < bod - > getNumLinks ( ) ; b + + )
{
if ( bod - > getLink ( b ) . m_collider & & bod - > getLink ( b ) . m_collider - > getActivationState ( ) = = ISLAND_SLEEPING )
isSleeping = true ;
}
if ( ! isSleeping )
{
m_scratch_r . resize ( bod - > getNumLinks ( ) + 1 ) ;
m_scratch_v . resize ( bod - > getNumLinks ( ) + 1 ) ;
m_scratch_m . resize ( bod - > getNumLinks ( ) + 1 ) ;
bool isConstraintPass = false ;
{
if ( ! bod - > isUsingRK4Integration ( ) )
{
bod - > computeAccelerationsArticulatedBodyAlgorithmMultiDof ( m_solverInfo . m_timeStep ,
m_scratch_r , m_scratch_v , m_scratch_m , isConstraintPass ,
getSolverInfo ( ) . m_jointFeedbackInWorldSpace ,
getSolverInfo ( ) . m_jointFeedbackInJointFrame ) ;
}
else
{
btAssert ( " RK4Integration is not supported " ) ;
}
}
}
}
}
}
clearGravity ( ) ;
2020-01-08 17:05:43 +00:00
}
void btDeformableMultiBodyDynamicsWorld : : clearGravity ( )
{
2021-09-29 13:47:08 +00:00
BT_PROFILE ( " btMultiBody clearGravity " ) ;
// clear rigid body gravity
for ( int i = 0 ; i < m_nonStaticRigidBodies . size ( ) ; i + + )
{
btRigidBody * body = m_nonStaticRigidBodies [ i ] ;
if ( body - > isActive ( ) )
{
body - > clearGravity ( ) ;
}
}
// clear multibody gravity
for ( int i = 0 ; i < this - > m_multiBodies . size ( ) ; i + + )
{
btMultiBody * bod = m_multiBodies [ i ] ;
bool isSleeping = false ;
if ( bod - > getBaseCollider ( ) & & bod - > getBaseCollider ( ) - > getActivationState ( ) = = ISLAND_SLEEPING )
{
isSleeping = true ;
}
for ( int b = 0 ; b < bod - > getNumLinks ( ) ; b + + )
{
if ( bod - > getLink ( b ) . m_collider & & bod - > getLink ( b ) . m_collider - > getActivationState ( ) = = ISLAND_SLEEPING )
isSleeping = true ;
}
if ( ! isSleeping )
{
bod - > addBaseForce ( - m_gravity * bod - > getBaseMass ( ) ) ;
for ( int j = 0 ; j < bod - > getNumLinks ( ) ; + + j )
{
bod - > addLinkForce ( j , - m_gravity * bod - > getLinkMass ( j ) ) ;
}
}
}
2020-01-08 17:05:43 +00:00
}
void btDeformableMultiBodyDynamicsWorld : : beforeSolverCallbacks ( btScalar timeStep )
{
2021-09-29 13:47:08 +00:00
if ( 0 ! = m_internalTickCallback )
{
( * m_internalTickCallback ) ( this , timeStep ) ;
}
if ( 0 ! = m_solverCallback )
{
( * m_solverCallback ) ( m_internalTime , this ) ;
}
2020-01-08 17:05:43 +00:00
}
void btDeformableMultiBodyDynamicsWorld : : afterSolverCallbacks ( btScalar timeStep )
{
2021-09-29 13:47:08 +00:00
if ( 0 ! = m_solverCallback )
{
( * m_solverCallback ) ( m_internalTime , this ) ;
}
2020-01-08 17:05:43 +00:00
}
void btDeformableMultiBodyDynamicsWorld : : addForce ( btSoftBody * psb , btDeformableLagrangianForce * force )
{
2021-09-29 13:47:08 +00:00
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 ) ;
}
}
void btDeformableMultiBodyDynamicsWorld : : removeForce ( btSoftBody * psb , btDeformableLagrangianForce * force )
{
btAlignedObjectArray < btDeformableLagrangianForce * > & forces = m_deformableBodySolver - > m_objective - > m_lf ;
int removed_index = - 1 ;
for ( int i = 0 ; i < forces . size ( ) ; + + i )
{
if ( forces [ i ] - > getForceType ( ) = = force - > getForceType ( ) )
{
forces [ i ] - > removeSoftBody ( psb ) ;
if ( forces [ i ] - > m_softBodies . size ( ) = = 0 )
removed_index = i ;
break ;
}
}
if ( removed_index > = 0 )
forces . removeAtIndex ( removed_index ) ;
}
void btDeformableMultiBodyDynamicsWorld : : removeSoftBodyForce ( btSoftBody * psb )
{
btAlignedObjectArray < btDeformableLagrangianForce * > & forces = m_deformableBodySolver - > m_objective - > m_lf ;
for ( int i = 0 ; i < forces . size ( ) ; + + i )
{
forces [ i ] - > removeSoftBody ( psb ) ;
}
2020-01-08 17:05:43 +00:00
}
void btDeformableMultiBodyDynamicsWorld : : removeSoftBody ( btSoftBody * body )
{
2021-09-29 13:47:08 +00:00
removeSoftBodyForce ( body ) ;
m_softBodies . remove ( body ) ;
btCollisionWorld : : removeCollisionObject ( body ) ;
// force a reinitialize so that node indices get updated.
m_deformableBodySolver - > reinitialize ( m_softBodies , btScalar ( - 1 ) ) ;
2020-01-08 17:05:43 +00:00
}
void btDeformableMultiBodyDynamicsWorld : : removeCollisionObject ( btCollisionObject * collisionObject )
{
2021-09-29 13:47:08 +00:00
btSoftBody * body = btSoftBody : : upcast ( collisionObject ) ;
if ( body )
removeSoftBody ( body ) ;
else
btDiscreteDynamicsWorld : : removeCollisionObject ( collisionObject ) ;
2020-01-08 17:05:43 +00:00
}
int btDeformableMultiBodyDynamicsWorld : : stepSimulation ( btScalar timeStep , int maxSubSteps , btScalar fixedTimeStep )
{
2021-09-29 13:47:08 +00:00
startProfiling ( timeStep ) ;
int numSimulationSubSteps = 0 ;
if ( maxSubSteps )
{
//fixed timestep with interpolation
m_fixedTimeStep = fixedTimeStep ;
m_localTime + = timeStep ;
if ( m_localTime > = fixedTimeStep )
{
numSimulationSubSteps = int ( m_localTime / fixedTimeStep ) ;
m_localTime - = numSimulationSubSteps * fixedTimeStep ;
}
}
else
{
//variable timestep
fixedTimeStep = timeStep ;
m_localTime = m_latencyMotionStateInterpolation ? 0 : timeStep ;
m_fixedTimeStep = 0 ;
if ( btFuzzyZero ( timeStep ) )
{
numSimulationSubSteps = 0 ;
maxSubSteps = 0 ;
}
else
{
numSimulationSubSteps = 1 ;
maxSubSteps = 1 ;
}
}
//process some debugging flags
if ( getDebugDrawer ( ) )
{
btIDebugDraw * debugDrawer = getDebugDrawer ( ) ;
gDisableDeactivation = ( debugDrawer - > getDebugMode ( ) & btIDebugDraw : : DBG_NoDeactivation ) ! = 0 ;
}
if ( numSimulationSubSteps )
{
//clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
int clampedSimulationSteps = ( numSimulationSubSteps > maxSubSteps ) ? maxSubSteps : numSimulationSubSteps ;
saveKinematicState ( fixedTimeStep * clampedSimulationSteps ) ;
for ( int i = 0 ; i < clampedSimulationSteps ; i + + )
{
internalSingleStepSimulation ( fixedTimeStep ) ;
synchronizeMotionStates ( ) ;
}
}
else
{
synchronizeMotionStates ( ) ;
}
clearForces ( ) ;
2020-01-08 17:05:43 +00:00
# ifndef BT_NO_PROFILE
2021-09-29 13:47:08 +00:00
CProfileManager : : Increment_Frame_Counter ( ) ;
2020-01-08 17:05:43 +00:00
# endif //BT_NO_PROFILE
2021-09-29 13:47:08 +00:00
return numSimulationSubSteps ;
2020-01-08 17:05:43 +00:00
}