void SymplecticEulerMap::flow( const VectorXs& q0, const VectorXs& v0, FlowableSystem& fsys, const unsigned iteration, const scalar& dt, VectorXs& q1, VectorXs& v1 )
{
assert( iteration > 0 );
const scalar start_time{ ( iteration - 1 ) * dt };
// Use q1 as temporary storage for the force
fsys.computeForce( q0, v0, start_time, q1 );
// Zero the force for fixed bodies
const unsigned nbodies{ fsys.numBodies() };
for( unsigned bdy_idx = 0; bdy_idx < nbodies; ++bdy_idx )
{
if( fsys.isKinematicallyScripted( bdy_idx ) )
{
q1.segment<3>( 3 * bdy_idx ).setZero();
}
}
// Velocity update
v1 = v0 + dt * fsys.Minv() * q1;
// Position update
q1 = q0 + dt * v1;
}
// TODO: Implement in a cleaner way -- create a function in fsys that checks if kinematic constraints are respected
bool ImpactFrictionMap::noImpulsesToKinematicGeometry( const FlowableSystem& fsys, const SparseMatrixsc& N, const VectorXs& alpha, const SparseMatrixsc& D, const VectorXs& beta, const VectorXs& v0 )
{
const VectorXs dv{ fsys.Minv() * ( N * alpha + D * beta ) };
assert( unsigned( v0.size() ) % fsys.numVelDoFsPerBody() == 0 );
const unsigned nbodies = unsigned( v0.size() ) / fsys.numVelDoFsPerBody();
for( unsigned i = 0; i < nbodies; ++i )
{
if( fsys.isKinematicallyScripted( i ) )
{
if( fsys.numVelDoFsPerBody() == 6 )
{
// 6-DoF rigid body in 3D
if( ( dv.segment<3>( 3 * i ).array() != 0.0 ).any() )
{
return false;
}
if( ( dv.segment<3>( 3 * nbodies + 3 * i ).array() != 0.0 ).any() )
{
return false;
}
}
else if( fsys.numVelDoFsPerBody() == 2 )
{
// 2-DoF ball in 2D
if( ( dv.segment<2>( 2 * i ).array() != 0.0 ).any() )
{
return false;
}
}
else
{
std::cerr << "Unhandled code path in ImpactFrictionMap::noImpulsesToKinematicGeometry. This is a bug." << std::endl;
std::exit( EXIT_FAILURE );
}
}
}
return true;
}
void GeometricImpactFrictionMap::flow( ScriptingCallback& call_back, FlowableSystem& fsys, ConstrainedSystem& csys, UnconstrainedMap& umap, FrictionSolver& friction_solver, const unsigned iteration, const scalar& dt, const scalar& CoR_default, const scalar& mu_default, const VectorXs& q0, const VectorXs& v0, VectorXs& q1, VectorXs& v1 )
{
// TODO: Sanity check input sizes
// TODO: Do something more elegant when the number of bodies changes due to insertions and deletions, like registering the maps with the scripting callbacks so they can update the cache.
// Initialize the friction impulse cache
if( m_f.size() != v0.size() )
{
m_f = VectorXs::Zero( v0.size() );
}
// Compute an unconstrained predictor step
umap.flow( q0, v0, fsys, iteration, dt, q1, v1 );
// Using the configuration at the predictor step, compute the set of active constraints
std::vector<std::unique_ptr<Constraint>> active_set;
csys.computeActiveSet( q0, q1, v0, active_set );
// If there are no active constraints, there is no need to perform collision response
if( active_set.empty() )
{
csys.clearConstraintCache();
if( m_write_constraint_forces )
{
exportConstraintForcesToBinary( q0, active_set, MatrixXXsc{ fsys.ambientSpaceDimensions(), 0 }, VectorXs::Zero(0), VectorXs::Zero(0), dt );
}
m_write_constraint_forces = false;
m_constraint_force_stream = nullptr;
return;
}
const unsigned ncollisions{ static_cast<unsigned>( active_set.size() ) };
// Pre-compute the full contact basis
MatrixXXsc contact_bases;
csys.computeContactBases( q0, v0, active_set, contact_bases );
assert( contact_bases.rows() == fsys.ambientSpaceDimensions() );
assert( contact_bases.cols() == fsys.ambientSpaceDimensions() * ncollisions );
// Set the coefficients of friction to the default
VectorXs mu{ VectorXs::Constant( ncollisions, mu_default ) };
// If scripting is enabled, use the scripted version
call_back.frictionCoefficientCallback( active_set, mu );
assert( ( mu.array() >= 0.0 ).all() );
// Coefficients of restitution
VectorXs CoR{ VectorXs::Constant( ncollisions, CoR_default ) };
// If scripting is enabled, use the scripted version
call_back.restitutionCoefficientCallback( active_set, CoR );
assert( ( CoR.array() >= 0.0 ).all() ); assert( ( CoR.array() <= 1.0 ).all() );
// Normal impulse magnitudes
VectorXs alpha{ ncollisions };
// Friction impulses magnitudes
VectorXs beta{ friction_solver.numFrictionImpulsesPerNormal( fsys.ambientSpaceDimensions() ) * ncollisions };
initializeImpulses( m_impulses_to_cache, fsys.ambientSpaceDimensions(), active_set, csys, alpha, beta );
// Compute the initial momentum and angular momentum
#ifndef NDEBUG
const bool momentum_should_be_conserved{ constraintSetShouldConserveMomentum( active_set ) };
VectorXs p0;
if( momentum_should_be_conserved ) { fsys.computeMomentum( v0, p0 ); }
const bool angular_momentum_should_be_conserved{ constraintSetShouldConserveAngularMomentum( active_set ) };
VectorXs L0;
if( angular_momentum_should_be_conserved ) { fsys.computeAngularMomentum( v0, L0 ); }
#endif
// Perform the coupled impact/friction solve
VectorXs v2{ v0.size() };
{
scalar error;
bool solve_succeeded;
friction_solver.solve( iteration, dt, fsys, fsys.M(), fsys.Minv(), CoR, mu, q0, v0, active_set, contact_bases, m_max_iters, m_abs_tol, m_f, alpha, beta, v2, solve_succeeded, error );
assert( error >= 0.0 );
if( !solve_succeeded )
{
std::cerr << "Warning, coupled impact/friction solve exceeded max iterations " << m_max_iters;
std::cerr << " with absolute error " << error << " stepping to time " << iteration * dt << std::endl;
}
}
// Cache the impulses, verify the expected uncached impulses
// Note: temporarily disabled test as it requires impulse caching support for all collision types
//#ifndef NDEBUG
//{
// cacheImpulses( ImpulsesToCache::NONE, fsys.ambientSpaceDimensions(), active_set, csys, alpha, beta );
// VectorXs alpha_test{ alpha.size() };
// VectorXs beta_test{ beta.size() };
// initializeImpulses( ImpulsesToCache::NONE, fsys.ambientSpaceDimensions(), active_set, csys, alpha_test, beta_test );
// assert( ( alpha_test.array() == 0.0 ).all() );
// assert( ( beta_test.array() == 0.0 ).all() );
//}
//{
// cacheImpulses( ImpulsesToCache::NORMAL, fsys.ambientSpaceDimensions(), active_set, csys, alpha, beta );
// VectorXs alpha_test{ alpha.size() };
// VectorXs beta_test{ beta.size() };
// initializeImpulses( ImpulsesToCache::NORMAL, fsys.ambientSpaceDimensions(), active_set, csys, alpha_test, beta_test );
// assert( ( alpha_test.array() == alpha.array() ).all() );
// assert( ( beta_test.array() == 0.0 ).all() );
//}
//{
// cacheImpulses( ImpulsesToCache::NORMAL_AND_FRICTION, fsys.ambientSpaceDimensions(), active_set, csys, alpha, beta );
// VectorXs alpha_test{ alpha.size() };
// VectorXs beta_test{ beta.size() };
// initializeImpulses( ImpulsesToCache::NORMAL_AND_FRICTION, fsys.ambientSpaceDimensions(), active_set, csys, alpha_test, beta_test );
// assert( ( alpha_test.array() == alpha.array() ).all() );
// assert( ( beta_test.array() == beta.array() ).all() );
//}
//#endif
// For the special case mu == 0, friction should be zero
#ifndef NDEBUG
{
const unsigned friction_impulses_per_collision{ friction_solver.numFrictionImpulsesPerNormal( fsys.ambientSpaceDimensions() ) };
for( unsigned col_idx = 0; col_idx < ncollisions; ++col_idx )
{
if( mu(col_idx) == 0.0 )
{
assert( ( beta.segment( friction_impulses_per_collision, col_idx * friction_impulses_per_collision ).array() == 0.0 ).all() );
}
}
}
#endif
// Verify that momentum and angular momentum are conserved
#ifndef NDEBUG
if( momentum_should_be_conserved )
{
VectorXs p1;
if( momentum_should_be_conserved ) { fsys.computeMomentum( v2, p1 ); }
assert( ( p0 - p1 ).lpNorm<Eigen::Infinity>() <= 1.0e-6 );
}
if( angular_momentum_should_be_conserved )
{
VectorXs L1;
if( angular_momentum_should_be_conserved ) { fsys.computeAngularMomentum( v2, L1 ); }
assert( ( L0 - L1 ).lpNorm<Eigen::Infinity>() <= 1.0e-6 );
}
#endif
// TODO: Derive bounds for gdotN and gdotD != 0.0
// Verify that the total energy decreased
//if( ( gdotN.array() == 0.0 ).all() && ( gdotD.array() == 0.0 ).all() )
//{
// const scalar T_init = v0.dot( fsys.M() * v0 );
// const scalar T_final = v2.dot( fsys.M() * v2 );
// if( T_final > T_init + 1.0e-6 )
// {
// std::cerr << "WARNING, energy increase detected in GeometricImpactFrictionMap: " << T_final - T_init << " error: " << error << std::endl;
// }
//}
// Sanity check: no impulses should apply to kinematic geometry
//assert( ImpactFrictionMap::noImpulsesToKinematicGeometry( fsys, N, alpha, D, beta, v0 ) );
// Cache the constraints for warm starting
cacheImpulses( m_impulses_to_cache, fsys.ambientSpaceDimensions(), active_set, csys, alpha, beta );
// Export constraint forces, if requested
if( m_write_constraint_forces )
{
exportConstraintForcesToBinary( q0, active_set, contact_bases, alpha, beta, dt );
}
m_write_constraint_forces = false;
m_constraint_force_stream = nullptr;
// Using the initial configuration and the new velocity, compute the final state
umap.flow( q0, v2, fsys, iteration, dt, q1, v1 );
}
