void StaticPlaneSphereConstraint::computeContactBasis( const VectorXs& q, const VectorXs& v, MatrixXXsc& basis ) const
{
const Vector3s n{ m_plane.n() };
assert( fabs( n.norm() - 1.0 ) <= 1.0e-6 );
// Compute the relative velocity to use as a direction for the tangent sample
Vector3s s{ computeRelativeVelocity( q, v ) };
// If the relative velocity is zero, any vector will do
if( n.cross( s ).squaredNorm() < 1.0e-9 )
{
s = FrictionUtilities::orthogonalVector( n );
}
// Otherwise project out the component along the normal and normalize the relative velocity
else
{
s = ( s - s.dot( n ) * n ).normalized();
}
// Invert the tangent vector in order to oppose
s *= -1.0;
// Create a second orthogonal sample in the tangent plane
const Vector3s t{ n.cross( s ).normalized() }; // Don't need to normalize but it won't hurt
assert( MathUtilities::isRightHandedOrthoNormal( n, s, t, 1.0e-6 ) );
basis.resize( 3, 3 );
basis.col( 0 ) = n;
basis.col( 1 ) = s;
basis.col( 2 ) = t;
}
void TeleportedCircleCircleConstraint::computeContactBasis( const VectorXs& q, const VectorXs& v, MatrixXXsc& basis ) const
{
assert( fabs( m_n.norm() - 1.0 ) <= 1.0e-6 );
const Vector2s t{ -m_n.y(), m_n.x() };
assert( fabs( t.norm() - 1.0 ) <= 1.0e-6 ); assert( fabs( m_n.dot( t ) ) <= 1.0e-6 );
basis.resize( 2, 2 );
basis.col( 0 ) = m_n;
basis.col( 1 ) = t;
}
void Constraint::evalKinematicRelVelGivenBases( const VectorXs& q, const VectorXs& v, const std::vector<std::unique_ptr<Constraint>>& constraints, const MatrixXXsc& bases, VectorXs& nrel, VectorXs& drel )
{
assert( bases.cols() == nrel.size() + drel.size() );
assert( nrel.size() % constraints.size() == 0 );
assert( drel.size() % constraints.size() == 0 );
// Number of constraints in the system
const unsigned ncons{ static_cast<unsigned>( constraints.size() ) };
// Number of tangent friction samples per constraint in the system
const unsigned friction_vectors_per_con{ static_cast<unsigned>( drel.size() / ncons ) };
for( unsigned con_num = 0; con_num < ncons; ++con_num )
{
// Grab the kinematic relative velocity
const VectorXs kinematic_rel_vel{ constraints[con_num]->computeKinematicRelativeVelocity( q, v ) };
assert( kinematic_rel_vel.size() == bases.rows() );
// Compute the column of the normal in the bases matrix
const unsigned n_idx{ ( friction_vectors_per_con + 1 ) * con_num };
assert( n_idx < bases.cols() ); assert( fabs( bases.col( n_idx ).norm() - 1.0 ) <= 1.0e-6 );
// Project the relative velocity onto the normal
nrel( con_num ) = - kinematic_rel_vel.dot( bases.col( n_idx ) );
// For each tangent friction sample
for( unsigned friction_sample = 0; friction_sample < friction_vectors_per_con; ++friction_sample )
{
// Compute the column of the current friction sample in the bases matrix
const unsigned f_idx{ ( friction_vectors_per_con + 1 ) * con_num + friction_sample + 1 };
assert( f_idx < bases.cols() );
assert( fabs( bases.col( f_idx ).norm() - 1.0 ) <= 1.0e-6 );
assert( fabs( bases.col( n_idx ).dot( bases.col( f_idx ) ) ) <= 1.0e-6 );
drel( friction_vectors_per_con * con_num + friction_sample ) = - kinematic_rel_vel.dot( bases.col( f_idx ) );
}
}
}
void RigidBody2DSim::computeImpactBases( const VectorXs& q, const std::vector<std::unique_ptr<Constraint>>& active_set, MatrixXXsc& impact_bases ) const
{
const unsigned ncols{ static_cast<unsigned>( active_set.size() ) };
impact_bases.resize( 2, ncols );
for( unsigned col_num = 0; col_num < ncols; ++col_num )
{
VectorXs current_normal;
active_set[col_num]->getWorldSpaceContactNormal( q, current_normal );
assert( fabs( current_normal.norm() - 1.0 ) <= 1.0e-6 );
impact_bases.col( col_num ) = current_normal;
}
}
void TeleportedCircleCircleConstraint::evalH( const VectorXs& q, const MatrixXXsc& basis, MatrixXXsc& H0, MatrixXXsc& H1 ) const
{
assert( H0.rows() == 2 ); assert( H0.cols() == 3 );
assert( H1.rows() == 2 ); assert( H1.cols() == 3 );
assert( ( basis * basis.transpose() - MatrixXXsc::Identity( 2, 2 ) ).lpNorm<Eigen::Infinity>() <= 1.0e-6 );
assert( fabs( basis.determinant() - 1.0 ) <= 1.0e-6 );
// Grab the contact normal
const Vector2s n{ basis.col( 0 ) };
// Grab the tangent basis
const Vector2s t{ basis.col( 1 ) };
// Format for H:
// n^T r x n
// t^T r x t
H0.block<1,2>(0,0) = n;
assert( fabs( MathUtilities::cross( m_r0, n ) ) <= 1.0e-6 );
H0(0,2) = 0.0;
H0.block<1,2>(1,0) = t;
H0(1,2) = MathUtilities::cross( m_r0, t );
H1.block<1,2>(0,0) = n;
assert( fabs( MathUtilities::cross( m_r1, n ) ) <= 1.0e-6 );
H1(0,2) = 0.0;
H1.block<1,2>(1,0) = t;
H1(1,2) = MathUtilities::cross( m_r1, t );
}
void MathUtilities::convertDenseToSparse( const bool filter_zeros, const MatrixXXsc& dense_matrix, SparseMatrixsc& sparse_matrix )
{
std::vector<Eigen::Triplet<scalar>> triplets;
for( int row = 0; row < dense_matrix.rows(); ++row )
{
for( int col = 0; col < dense_matrix.cols(); ++col )
{
if( dense_matrix( row, col ) != 0.0 || !filter_zeros )
{
triplets.emplace_back( Eigen::Triplet<scalar>{ row, col, dense_matrix( row, col ) } );
}
}
}
sparse_matrix.resize( dense_matrix.rows(), dense_matrix.cols() );
sparse_matrix.setFromTriplets( std::begin( triplets ), std::end( triplets ) );
sparse_matrix.makeCompressed();
}
// TODO: Despecialize from smooth
void FrictionOperator::formGeneralizedSmoothFrictionBasis( const unsigned ndofs, const unsigned ncons, const VectorXs& q, const std::vector<std::unique_ptr<Constraint>>& K, const MatrixXXsc& bases, SparseMatrixsc& D )
{
assert( ncons == K.size() );
const unsigned nambientdims{ static_cast<unsigned>( bases.rows() ) };
const unsigned nsamples{ nambientdims - 1 };
D.resize( ndofs, nsamples * ncons );
auto itr = K.cbegin();
{
VectorXi column_nonzeros( D.cols() );
for( unsigned collision_number = 0; collision_number < ncons; ++collision_number )
{
for( unsigned sample_number = 0; sample_number < nsamples; ++sample_number )
{
assert( nsamples * collision_number + sample_number < column_nonzeros.size() );
column_nonzeros( nsamples * collision_number + sample_number ) = (*itr)->frictionStencilSize();
}
++itr;
}
assert( ( column_nonzeros.array() > 0 ).all() );
assert( itr == K.cend() );
D.reserve( column_nonzeros );
}
itr = K.cbegin();
for( unsigned collision_number = 0; collision_number < ncons; ++collision_number )
{
for( unsigned sample_number = 0; sample_number < nsamples; ++sample_number )
{
const unsigned current_column{ nsamples * collision_number + sample_number };
const VectorXs current_sample{ bases.col( nambientdims * collision_number + sample_number + 1 ) };
assert( fabs( current_sample.dot( bases.col( nambientdims * collision_number ) ) ) <= 1.0e-6 );
(*itr)->computeGeneralizedFrictionGivenTangentSample( q, current_sample, current_column, D );
}
++itr;
}
assert( itr == K.cend() );
D.prune( []( const Eigen::Index& row, const Eigen::Index& col, const scalar& value ) { return value != 0.0; } );
assert( D.innerNonZeroPtr() == nullptr );
}
void Constraint::computeBasis( const VectorXs& q, const VectorXs& v, MatrixXXsc& basis ) const
{
computeContactBasis( q, v, basis );
assert( basis.rows() == basis.cols() );
assert( ( basis * basis.transpose() - MatrixXXsc::Identity( basis.rows(), basis.cols() ) ).lpNorm<Eigen::Infinity>() <= 1.0e-6 );
assert( fabs( basis.determinant() - 1.0 ) <= 1.0e-6 );
}
void StaticPlaneSphereConstraint::evalH( const VectorXs& q, const MatrixXXsc& basis, MatrixXXsc& H0, MatrixXXsc& H1 ) const
{
assert( H0.rows() == 3 );
assert( H0.cols() == 6 );
assert( H1.rows() == 3 );
assert( H1.cols() == 6 );
// Grab the contact normal
const Vector3s n{ basis.col( 0 ) };
// Grab the tangent basis
const Vector3s s{ basis.col( 1 ) };
const Vector3s t{ basis.col( 2 ) };
assert( MathUtilities::isRightHandedOrthoNormal( n, s, t, 1.0e-6 ) );
// Compute the displacement from the center of mass to the point of contact
assert( m_r >= 0.0 );
const Vector3s r_world{ - m_r * n };
H0.block<1,3>(0,0) = n;
H0.block<1,3>(0,3).setZero();
H0.block<1,3>(1,0) = s;
H0.block<1,3>(1,3) = r_world.cross( s );
H0.block<1,3>(2,0) = t;
H0.block<1,3>(2,3) = r_world.cross( t );
}
void LCPOperatorQL::flow( const std::vector<std::unique_ptr<Constraint>>& cons, const SparseMatrixsc& M, const SparseMatrixsc& Minv, const VectorXs& q0, const VectorXs& v0, const VectorXs& v0F, const SparseMatrixsc& N, const SparseMatrixsc& Q, const VectorXs& nrel, const VectorXs& CoR, VectorXs& alpha )
{
// Q in 1/2 \alpha^T Q \alpha
assert( Q.rows() == Q.cols() );
MatrixXXsc Qdense = Q;
// Linear term in the objective
VectorXs Adense;
ImpactOperatorUtilities::computeLCPQPLinearTerm( N, nrel, CoR, v0, v0F, Adense );
// Solve the QP
assert( Qdense.rows() == Adense.size() ); assert( Adense.size() == alpha.size() );
const int status = solveQP( m_tol, Qdense, Adense, alpha );
// Check for problems
if( 0 != status )
{
std::cerr << "Warning, failed to solve QP in LCPOperatorQL::flow: " << QLUtilities::QLReturnStatusToString(status) << std::endl;
}
// TODO: Sanity check the solution here
}
// TODO: Unspecialize from 3D
void Constraint::computeNormalAndRelVelAlignedTangent( const VectorXs& q, const VectorXs& v, VectorXs& n, VectorXs& t, VectorXs& tangent_rel_vel ) const
{
MatrixXXsc basis;
computeBasis( q, v, basis );
assert( basis.rows() == basis.cols() ); assert( basis.rows() == 3 );
n = basis.col( 0 );
t = basis.col( 1 );
// Compute the relative velocity
tangent_rel_vel = computeRelativeVelocity( q, v );
// Project out normal component of relative velocity
tangent_rel_vel = tangent_rel_vel - tangent_rel_vel.dot( n ) * n;
#ifndef NDEBUG
// Relative velocity and tangent should be parallel
assert( Eigen::Map<Vector3s>( tangent_rel_vel.data() ).cross( Eigen::Map<Vector3s>( t.data() ) ).lpNorm<Eigen::Infinity>() <= 1.0e-6 );
// If tangent relative velocity is non-negligble, tangent should oppose
if( tangent_rel_vel.norm() > 1.0e-9 )
{
assert( fabs( tangent_rel_vel.normalized().dot( t ) + 1.0 ) <= 1.0e-6 );
}
#endif
}
void RigidBody2DSim::computeContactBases( const VectorXs& q, const VectorXs& v, const std::vector<std::unique_ptr<Constraint>>& active_set, MatrixXXsc& contact_bases ) const
{
const unsigned ncols{ static_cast<unsigned>( active_set.size() ) };
contact_bases.resize( 2, 2 * ncols );
for( unsigned col_num = 0; col_num < ncols; ++col_num )
{
MatrixXXsc basis;
active_set[col_num]->computeBasis( q, v, basis );
assert( basis.rows() == basis.cols() ); assert( basis.rows() == 2 );
assert( ( basis * basis.transpose() - MatrixXXsc::Identity( 2, 2 ) ).lpNorm<Eigen::Infinity>() <= 1.0e-6 );
assert( fabs( basis.determinant() - 1.0 ) <= 1.0e-6 );
contact_bases.block<2,2>( 0, 2 * col_num ) = basis;
}
}
void BodyBodyConstraint::evalH( const VectorXs& q, const MatrixXXsc& basis, MatrixXXsc& H0, MatrixXXsc& H1 ) const
{
assert( H0.rows() == 3 );
assert( H0.cols() == 6 );
assert( H1.rows() == 3 );
assert( H1.cols() == 6 );
assert( ( m_n - basis.col( 0 ) ).lpNorm<Eigen::Infinity>() <= 1.0e-6 );
// Grab the contact normal
const Vector3s n{ basis.col( 0 ) };
// Grab the tangent basis
const Vector3s s{ basis.col( 1 ) };
const Vector3s t{ basis.col( 2 ) };
assert( MathUtilities::isRightHandedOrthoNormal( n, s, t, 1.0e-6 ) );
// Format for H:
// n^T \tilde{n}^T
// s^T \tilde{s}^T
// t^T \tilde{t}^T
H0.block<1,3>(0,0) = n;
H0.block<1,3>(0,3) = m_r0.cross( n );
H0.block<1,3>(1,0) = s;
H0.block<1,3>(1,3) = m_r0.cross( s );
H0.block<1,3>(2,0) = t;
H0.block<1,3>(2,3) = m_r0.cross( t );
H1.block<1,3>(0,0) = n;
H1.block<1,3>(0,3) = m_r1.cross( n );
H1.block<1,3>(1,0) = s;
H1.block<1,3>(1,3) = m_r1.cross( s );
H1.block<1,3>(2,0) = t;
H1.block<1,3>(2,3) = m_r1.cross( t );
}
static int solveQP( const scalar& tol, MatrixXXsc& C, VectorXs& dvec, VectorXs& alpha )
{
static_assert( std::is_same<scalar,double>::value, "QL only supports doubles." );
assert( dvec.size() == alpha.size() );
// All constraints are bound constraints.
int m = 0;
int me = 0;
int mmax = 0;
// C should be symmetric
assert( ( C - C.transpose() ).lpNorm<Eigen::Infinity>() < 1.0e-14 );
// Number of degrees of freedom.
assert( C.rows() == C.cols() );
int n{ int( C.rows() ) };
int nmax = n;
int mnn = m + n + n;
assert( dvec.size() == nmax );
// Impose non-negativity constraints on all variables
Eigen::VectorXd xl = Eigen::VectorXd::Zero( nmax );
Eigen::VectorXd xu = Eigen::VectorXd::Constant( nmax, std::numeric_limits<double>::infinity() );
// u will contain the constraint multipliers
Eigen::VectorXd u( mnn );
// Status of the solve
int ifail = -1;
// Use the built-in cholesky decomposition
int mode = 1;
// Some fortran output stuff
int iout = 0;
// 1 => print output, 0 => silent
int iprint = 1;
// Working space
assert( m == 0 && me == 0 && mmax == 0 );
int lwar = 3 * ( nmax * nmax ) / 2 + 10 * nmax + 2;
Eigen::VectorXd war( lwar );
// Additional working space
int liwar = n;
Eigen::VectorXi iwar( liwar );
{
scalar tol_local = tol;
ql_( &m,
&me,
&mmax,
&n,
&nmax,
&mnn,
C.data(),
dvec.data(),
nullptr,
nullptr,
xl.data(),
xu.data(),
alpha.data(),
u.data(),
&tol_local,
&mode,
&iout,
&ifail,
&iprint,
war.data(),
&lwar,
iwar.data(),
&liwar );
}
return ifail;
}
void Constraint::computeForcingTerm( const VectorXs& q, const VectorXs& v, const MatrixXXsc& basis, const scalar& CoR, const scalar& nrel, const VectorXs& drel, VectorXs& constant_term ) const
{
assert( basis.rows() == basis.cols() );
assert( ( basis * basis.transpose() - MatrixXXsc::Identity( basis.rows(), basis.cols() ) ).lpNorm<Eigen::Infinity>() <= 1.0e-6 );
assert( fabs( basis.determinant() - 1.0 ) <= 1.0e-6 );
assert( CoR >= 0.0 ); assert( CoR <= 1.0 );
constant_term = VectorXs::Zero( basis.rows() );
assert( fabs( evalNdotV( q, v ) - basis.col(0).dot( computeRelativeVelocity( q, v ) ) ) <= 1.0e-6 );
//constant_term += basis.col(0) * ( CoR * ( evalNdotV( q, v ) - nrel ) + ( 1.0 + CoR ) * nrel );
constant_term += basis.col(0) * ( CoR * evalNdotV( q, v ) + nrel );
assert( drel.size() == basis.cols() - 1 );
for( unsigned friction_sample = 0; friction_sample < drel.size(); ++friction_sample )
{
constant_term += basis.col( friction_sample + 1 ) * drel( friction_sample );
}
}
MatrixXXsc Constraint::computeFrictionBasis( const VectorXs& q, const VectorXs& v ) const
{
MatrixXXsc basis;
computeBasis( q, v, basis );
return basis.block( 0, 1, basis.rows(), basis.cols() - 1 );
}
void ImpactFrictionMap::exportConstraintForcesToBinaryFile( const VectorXs& q, const std::vector<std::unique_ptr<Constraint>>& constraints, const MatrixXXsc& contact_bases, const VectorXs& alpha, const VectorXs& beta, const scalar& dt, HDF5File& output_file )
{
const unsigned ncons = constraints.size();
assert( ncons == alpha.size() );
assert( std::vector<std::unique_ptr<Constraint>>::size_type( ncons ) == constraints.size() );
assert( alpha.size() == ncons );
const unsigned ambient_space_dims{ static_cast<unsigned>( contact_bases.rows() ) };
assert( ambient_space_dims == 2 || ambient_space_dims == 3 );
assert( beta.size() == ncons * ( ambient_space_dims - 1 ) );
// Write out the number of collisions
output_file.writeScalar( "", "collision_count", ncons );
// NB: Prior to version 1.8.7, HDF5 did not support zero sized dimensions.
// Some versions of Ubuntu still have old versions of HDF5, so workaround.
// TODO: Remove once our servers are updated.
if( ncons == 0 )
{
return;
}
// Write out the indices of all bodies involved
{
// Place all indices into a single matrix for output
Matrix2Xic collision_indices{ 2, ncons };
for( unsigned con = 0; con < ncons; ++con )
{
std::pair<int,int> indices;
assert( constraints[con] != nullptr );
getCollisionIndices( *constraints[con], indices );
collision_indices.col( con ) << indices.first, indices.second;
}
// Output the indices
output_file.writeMatrix( "", "collision_indices", collision_indices );
}
// Write out the world space contact points
{
// Place all contact points into a single matrix for output
MatrixXXsc collision_points{ ambient_space_dims, ncons };
for( unsigned con = 0; con < ncons; ++con )
{
VectorXs contact_point;
assert( constraints[con] != nullptr );
constraints[con]->getWorldSpaceContactPoint( q, contact_point );
assert( contact_point.size() == ambient_space_dims );
collision_points.col( con ) = contact_point;
}
// Output the points
output_file.writeMatrix( "", "collision_points", collision_points );
}
// Write out the world space contact normals
{
// Place all contact normals into a single matrix for output
MatrixXXsc collision_normals{ ambient_space_dims, ncons };
for( unsigned con = 0; con < ncons; ++con )
{
collision_normals.col( con ) = contact_bases.col( ambient_space_dims * con );
}
#ifndef NDEBUG
for( unsigned con = 0; con < ncons; ++con )
{
assert( fabs( collision_normals.col( con ).norm() - 1.0 ) <= 1.0e-6 );
}
#endif
// Output the normals
output_file.writeMatrix( "", "collision_normals", collision_normals );
}
// Write out the collision forces
{
// Place all contact forces into a single matrix for output
MatrixXXsc collision_forces{ ambient_space_dims, ncons };
for( unsigned con = 0; con < ncons; ++con )
{
// Contribution from normal
collision_forces.col( con ) = alpha( con ) * contact_bases.col( ambient_space_dims * con );
// Contribution from friction
for( unsigned friction_sample = 0; friction_sample < ambient_space_dims - 1; ++friction_sample )
{
assert( ( ambient_space_dims - 1 ) * con + friction_sample < beta.size() );
const scalar impulse{ beta( ( ambient_space_dims - 1 ) * con + friction_sample ) };
const unsigned column_number{ ambient_space_dims * con + friction_sample + 1 };
assert( column_number < contact_bases.cols() );
assert( fabs( contact_bases.col( ambient_space_dims * con ).dot( contact_bases.col( column_number ) ) ) <= 1.0e-6 );
collision_forces.col( con ) += impulse * contact_bases.col( column_number );
}
}
// Output the forces
output_file.writeMatrix( "", "collision_forces", collision_forces );
}
}
// TODO: Don't do the if here, have children do what they need to do
scalar Constraint::computeLambda( const VectorXs& q, const VectorXs& v ) const
{
MatrixXXsc basis;
computeBasis( q, v, basis );
assert( basis.rows() == basis.cols() );
assert( basis.cols() == 2 || basis.cols() == 3 );
const VectorXs rel_vel = computeRelativeVelocity( q, v );
assert( rel_vel.size() == basis.rows() );
if( basis.cols() == 3 )
{
const Vector2s tangent_vel( rel_vel.dot( basis.col( 1 ) ), rel_vel.dot( basis.col( 2 ) ) );
return tangent_vel.norm();
}
else if( basis.cols() == 2 )
{
return fabs( rel_vel.dot( basis.col( 1 ) ) );
}
std::cerr << "Unhandled case in Constraint::computeLambda" << std::endl;
std::exit( EXIT_FAILURE );
}
