// 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 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;
}
