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 ) );
}
}
}
// Adds the gradient of the penalty potential (-1 * force) for a particle-
// half-plane pair to the total.
// Read the positions of the particle from the input variable x.
// Inputs:
// x: The positions of the particles in the scene.
// vidx: The index of the particle.
// pidx: The index of the half-plane, i.e. the position and normal vectors
// for the half-plane can be retrieved by calling
// m_scene.getHalfplane(pidx).
// Outputs:
// gradE: The total gradient of the penalty force. *ADD* the particle-
// half-plane gradient to this total gradient.
void PenaltyForce::addParticleHalfplaneGradEToTotal(const VectorXs &x, int vidx, int pidx, VectorXs &gradE)
{
// VectorXs x1 = x.segment<2>(2*vidx);
// VectorXs nh = m_scene.getHalfplane(pidx).second;
//
// // your implementation here
//
VectorXs x1 = x.segment<2>(2*vidx);
VectorXs nh = m_scene.getHalfplane(pidx).second;
VectorXs n = (m_scene.getHalfplane(pidx).first - x1).dot(nh)/(nh.dot(nh))*nh;
VectorXs nhat = n;
nhat.normalize();
double r = m_scene.getRadius(vidx);
if(n.norm() < r + m_thickness)
{
gradE.segment<2>(2*vidx) -= m_k*(n.norm()-r - m_thickness)*nhat.dot(nh)/(nh.dot(nh))*nh;
}
}
// 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 );
}
static VectorXs parallelTransport2D( const VectorXs& n0, const VectorXs& n1, const VectorXs& t0 )
{
assert( fabs( n0.norm() - 1.0 ) <= 1.0e-6 ); assert( fabs( n1.norm() - 1.0 ) <= 1.0e-6 );
// x is cos of angle, y is sin of angle
const Vector2s r{ n0.dot( n1 ), MathUtilities::cross( n0, n1 ) };
assert( fabs( r.norm() - 1.0 ) <= 1.0e-6 );
// Rotate t0
VectorXs t1{ 2 };
t1( 0 ) = r.x() * t0.x() - r.y() * t0.y();
t1( 1 ) = r.y() * t0.x() + r.x() * t0.y();
assert( fabs( t0.norm() - t1.norm() ) <= 1.0e-6 );
// TODO: Assert n0,t0 angle is same as n1,t1 angle
return t1;
}
// 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 );
}
// 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
}