void BodyBodyConstraint::computeGeneralizedFrictionGivenTangentSample( const VectorXs& q, const VectorXs& t, const unsigned column, SparseMatrixsc& D ) const
{
assert( column < unsigned( D.cols() ) );
assert( q.size() % 12 == 0 );
assert( t.size() == 3 );
assert( fabs( t.norm() - 1.0 ) <= 1.0e-6 );
assert( fabs( m_n.dot( t ) ) <= 1.0e-6 );
assert( m_idx0 < m_idx1 );
const unsigned nbodies{ static_cast<unsigned>( q.size() / 12 ) };
// Effect on center of mass of body i
D.insert( 3 * m_idx0 + 0, column ) = t.x();
D.insert( 3 * m_idx0 + 1, column ) = t.y();
D.insert( 3 * m_idx0 + 2, column ) = t.z();
// Effect on orientation of body i
{
const Vector3s ntilde0{ m_r0.cross( Eigen::Map<const Vector3s>{ t.data() } ) };
D.insert( 3 * ( m_idx0 + nbodies ) + 0, column ) = ntilde0.x();
D.insert( 3 * ( m_idx0 + nbodies ) + 1, column ) = ntilde0.y();
D.insert( 3 * ( m_idx0 + nbodies ) + 2, column ) = ntilde0.z();
}
// Effect on center of mass of body j
D.insert( 3 * m_idx1 + 0, column ) = - t.x();
D.insert( 3 * m_idx1 + 1, column ) = - t.y();
D.insert( 3 * m_idx1 + 2, column ) = - t.z();
// Effect on orientation of body j
{
const Vector3s ntilde1{ m_r1.cross( Eigen::Map<const Vector3s>{ t.data() } ) };
D.insert( 3 * ( m_idx1 + nbodies ) + 0, column ) = - ntilde1.x();
D.insert( 3 * ( m_idx1 + nbodies ) + 1, column ) = - ntilde1.y();
D.insert( 3 * ( m_idx1 + nbodies ) + 2, column ) = - ntilde1.z();
}
}
void StaticPlaneSphereConstraint::computeGeneralizedFrictionGivenTangentSample( const VectorXs& q, const VectorXs& t, const unsigned column, SparseMatrixsc& D ) const
{
assert( t.size() == 3 );
assert( column < unsigned( D.cols() ) );
assert( q.size() % 12 == 0 );
assert( fabs( t.norm() - 1.0 ) <= 1.0e-6 );
assert( fabs( m_plane.n().dot( t ) ) <= 1.0e-6 );
// Effect on center of mass
D.insert( 3 * m_sphere_idx + 0, column ) = t.x();
D.insert( 3 * m_sphere_idx + 1, column ) = t.y();
D.insert( 3 * m_sphere_idx + 2, column ) = t.z();
// Effect on orientation
{
const unsigned nbodies{ static_cast<unsigned>( q.size() / 12 ) };
// Compute the displacement from the center of mass to the point of contact
assert( fabs( m_plane.n().norm() - 1.0 ) <= 1.0e-10 );
assert( m_r >= 0.0 );
const Vector3s r_world{ - m_r * m_plane.n() };
const Vector3s ntilde{ r_world.cross( Eigen::Map<const Vector3s>( t.data() ) ) };
D.insert( 3 * ( nbodies + m_sphere_idx ) + 0, column ) = ntilde.x();
D.insert( 3 * ( nbodies + m_sphere_idx ) + 1, column ) = ntilde.y();
D.insert( 3 * ( nbodies + m_sphere_idx ) + 2, column ) = ntilde.z();
}
}
// TODO: Use utility class here
void MathUtilities::deserialize( SparseMatrixsc& A, std::istream& stm )
{
assert( stm.good() );
VectorXi col_ptr;
VectorXi row_ind;
VectorXs val;
// Read the number of rows in the matrix
int rows{ -1 };
stm.read((char*)&rows,sizeof(int));
assert( rows >= 0 );
// Read the number of columns in the matrix
int cols{ -1 };
stm.read((char*)&cols,sizeof(int));
assert( cols >= 0 );
// Read the column pointer array
col_ptr.resize(cols+1);
stm.read((char*)col_ptr.data(),col_ptr.size()*sizeof(int));
// Read the number of nonzeros in the array
int nnz{ -1 };
stm.read((char*)&nnz,sizeof(int));
assert( nnz >= 0 );
// Read the row-index array
row_ind.resize(nnz);
stm.read((char*)row_ind.data(),row_ind.size()*sizeof(int));
// Read the value array
val.resize(nnz);
stm.read((char*)val.data(),val.size()*sizeof(scalar));
A.resize(rows,cols);
A.reserve(nnz);
for( int col = 0; col < cols; ++col )
{
A.startVec(col);
for( int curel = col_ptr(col); curel < col_ptr(col+1); ++curel )
{
int row = row_ind(curel);
scalar curval = val(curel);
A.insertBack(row,col) = curval;
}
}
A.finalize();
}
void MathUtilities::serialize( const SparseMatrixsc& A, std::ostream& stm )
{
assert( stm.good() );
VectorXi col_ptr;
VectorXi row_ind;
VectorXs val;
MathUtilities::extractDataCCS( A, col_ptr, row_ind, val );
assert( col_ptr.size() == A.cols() + 1 ); assert( row_ind.size() == A.nonZeros() ); assert( val.size() == A.nonZeros() );
// Size of col_ptr == A.cols() + 1
Utilities::serializeBuiltInType( A.rows(), stm );
Utilities::serializeBuiltInType( A.cols(), stm );
stm.write( reinterpret_cast<char*>( col_ptr.data() ), col_ptr.size() * sizeof(int) );
// Size of row_ind == size of val == A.nonZeros()
Utilities::serializeBuiltInType( A.nonZeros(), stm );
stm.write( reinterpret_cast<char*>( row_ind.data() ), row_ind.size() * sizeof(int) );
stm.write( reinterpret_cast<char*>( val.data() ), val.size() * sizeof(scalar) );
}
// 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
}
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;
}
