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;
}
// This method and the smooth version share the second half of code. Abstract that out.
void StaticPlaneSphereConstraint::computeGeneralizedFrictionDisk( const VectorXs& q, const VectorXs& v, const int start_column, const int num_samples, SparseMatrixsc& D, VectorXs& drel ) const
{
assert( start_column >= 0 );
assert( start_column < D.cols() );
assert( num_samples > 0 );
assert( start_column + num_samples - 1 < D.cols() );
assert( q.size() % 12 == 0 );
assert( q.size() == 2 * v.size() );
const Vector3s n{ m_plane.n() };
assert( fabs( n.norm() - 1.0 ) <= 1.0e-6 );
std::vector<Vector3s> friction_disk( static_cast<std::vector<Vector3s>::size_type>( num_samples ) );
assert( friction_disk.size() == std::vector<Vector3s>::size_type( num_samples ) );
{
// Compute the relative velocity
Vector3s tangent_suggestion{ computeRelativeVelocity( q, v ) };
if( tangent_suggestion.cross( n ).squaredNorm() < 1.0e-9 )
{
tangent_suggestion = FrictionUtilities::orthogonalVector( n );
}
tangent_suggestion *= -1.0;
// Sample the friction disk
FrictionUtilities::generateOrthogonalVectors( n, friction_disk, tangent_suggestion );
}
assert( unsigned( num_samples ) == friction_disk.size() );
// Compute the displacement from the center of mass to the point of contact
assert( fabs( n.norm() - 1.0 ) <= 1.0e-10 ); assert( m_r >= 0.0 );
const Vector3s r_world{ - m_r * n };
// Cache the velocity of the collision point on the plane
const Vector3s plane_collision_point_vel{ computePlaneCollisionPointVelocity( q ) };
// For each sample of the friction disk
const unsigned nbodies{ static_cast<unsigned>( q.size() / 12 ) };
for( unsigned friction_sample = 0; friction_sample < unsigned( num_samples ); ++friction_sample )
{
const unsigned cur_col{ start_column + friction_sample };
assert( cur_col < unsigned( D.cols() ) );
// Effect on center of mass
D.insert( 3 * m_sphere_idx + 0, cur_col ) = friction_disk[friction_sample].x();
D.insert( 3 * m_sphere_idx + 1, cur_col ) = friction_disk[friction_sample].y();
D.insert( 3 * m_sphere_idx + 2, cur_col ) = friction_disk[friction_sample].z();
// Effect on orientation
{
const Vector3s ntilde{ r_world.cross( friction_disk[friction_sample] ) };
D.insert( 3 * ( nbodies + m_sphere_idx ) + 0, cur_col ) = ntilde.x();
D.insert( 3 * ( nbodies + m_sphere_idx ) + 1, cur_col ) = ntilde.y();
D.insert( 3 * ( nbodies + m_sphere_idx ) + 2, cur_col ) = ntilde.z();
}
// Relative velocity contribution from kinematic scripting
assert( cur_col < drel.size() );
drel( cur_col ) = - friction_disk[friction_sample].dot( plane_collision_point_vel );
}
}
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 );
}
}
// 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 );
}
// 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 BodyBodyConstraint::computeGeneralizedFrictionDisk( const VectorXs& q, const VectorXs& v, const int start_column, const int num_samples, SparseMatrixsc& D, VectorXs& drel ) const
{
assert( start_column >= 0 );
assert( start_column < D.cols() );
assert( num_samples > 0 );
assert( start_column + num_samples - 1 < D.cols() );
assert( q.size() % 12 == 0 );
assert( q.size() == 2 * v.size() );
const unsigned nbodies{ static_cast<unsigned>( q.size() / 12 ) };
assert( fabs( m_n.norm() - 1.0 ) <= 1.0e-6 );
std::vector<Vector3s> friction_disk;
{
// Compute the relative velocity
Vector3s tangent_suggestion{ computeRelativeVelocity( q, v ) };
if( tangent_suggestion.cross( m_n ).squaredNorm() < 1.0e-9 )
{
tangent_suggestion = FrictionUtilities::orthogonalVector( m_n );
}
tangent_suggestion *= -1.0;
// Sample the friction disk
friction_disk.resize( num_samples );
FrictionUtilities::generateOrthogonalVectors( m_n, friction_disk, tangent_suggestion );
}
assert( unsigned( num_samples ) == friction_disk.size() );
// For each sample of the friction disk
assert( m_idx0 < m_idx1 );
for( int i = 0; i < num_samples; ++i )
{
const int cur_col{ start_column + i };
assert( cur_col >= 0 );
assert( cur_col < D.cols() );
// Effect on center of mass of body i
D.insert( 3 * m_idx0 + 0, cur_col ) = friction_disk[i].x();
D.insert( 3 * m_idx0 + 1, cur_col ) = friction_disk[i].y();
D.insert( 3 * m_idx0 + 2, cur_col ) = friction_disk[i].z();
// Effect on orientation of body i
{
const Vector3s ttilde0{ m_r0.cross( friction_disk[i] ) };
D.insert( 3 * ( m_idx0 + nbodies ) + 0, cur_col ) = ttilde0.x();
D.insert( 3 * ( m_idx0 + nbodies ) + 1, cur_col ) = ttilde0.y();
D.insert( 3 * ( m_idx0 + nbodies ) + 2, cur_col ) = ttilde0.z();
}
// Effect on center of mass of body j
D.insert( 3 * m_idx1 + 0, cur_col ) = - friction_disk[i].x();
D.insert( 3 * m_idx1 + 1, cur_col ) = - friction_disk[i].y();
D.insert( 3 * m_idx1 + 2, cur_col ) = - friction_disk[i].z();
// Effect on orientation of body j
{
const Vector3s ttilde1{ m_r1.cross( friction_disk[i] ) };
D.insert( 3 * ( m_idx1 + nbodies ) + 0, cur_col ) = - ttilde1.x();
D.insert( 3 * ( m_idx1 + nbodies ) + 1, cur_col ) = - ttilde1.y();
D.insert( 3 * ( m_idx1 + nbodies ) + 2, cur_col ) = - ttilde1.z();
}
// Relative velocity contribution from kinematic scripting
assert( cur_col < drel.size() );
// Zero for now
drel( cur_col ) = 0.0;
}
}
scalar BodyBodyConstraint::evalNdotV( const VectorXs& q, const VectorXs& v ) const
{
return m_n.dot( computeRelativeVelocity( q, v ) );
}
void BodyBodyConstraint::computeSmoothGeneralizedFrictionDisk( const VectorXs& q, const VectorXs& v, const int start_column, SparseMatrixsc& D ) const
{
assert( start_column >= 0 );
assert( start_column < D.cols() );
assert( start_column+1 < D.cols() );
assert( q.size() % 12 == 0 );
assert( q.size() == 2 * v.size() );
std::vector<Vector3s> friction_disk{ 2 };
// Compute the relative velocity to use as a direction for the tangent sample
friction_disk[0] = computeRelativeVelocity( q, v );
// If the relative velocity is zero, any vector will do
if( friction_disk[0].cross( m_n ).squaredNorm() < 1.0e-9 )
{
friction_disk[0] = FrictionUtilities::orthogonalVector( m_n );
}
// Otherwise project out the component along the normal and normalize the relative velocity
else
{
friction_disk[0] = ( friction_disk[0] - friction_disk[0].dot( m_n ) * m_n ).normalized();
}
// Invert the tangent vector in order to oppose
friction_disk[0] *= -1.0;
// Create a second orthogonal sample in the tangent plane
friction_disk[1] = m_n.cross( friction_disk[0] ).normalized(); // Don't need to normalize but it won't hurt
assert( MathUtilities::isRightHandedOrthoNormal( m_n, friction_disk[0], friction_disk[1], 1.0e-6 ) );
// For each sample of the friction disk
assert( m_idx0 < m_idx1 );
const unsigned nbodies{ static_cast<unsigned>( q.size() / 12 ) };
for( int i = 0; i < 2; ++i )
{
const int cur_col = start_column + i;
assert( cur_col >= 0 );
assert( cur_col < D.cols() );
// Effect on center of mass of body i
D.insert( 3 * m_idx0 + 0, cur_col ) = friction_disk[i].x();
D.insert( 3 * m_idx0 + 1, cur_col ) = friction_disk[i].y();
D.insert( 3 * m_idx0 + 2, cur_col ) = friction_disk[i].z();
// Effect on orientation of body i
{
const Vector3s ntilde0{ m_r0.cross( friction_disk[i] ) };
D.insert( 3 * ( m_idx0 + nbodies ) + 0, cur_col ) = ntilde0.x();
D.insert( 3 * ( m_idx0 + nbodies ) + 1, cur_col ) = ntilde0.y();
D.insert( 3 * ( m_idx0 + nbodies ) + 2, cur_col ) = ntilde0.z();
}
// Effect on center of mass of body j
D.insert( 3 * m_idx1 + 0, cur_col ) = - friction_disk[i].x();
D.insert( 3 * m_idx1 + 1, cur_col ) = - friction_disk[i].y();
D.insert( 3 * m_idx1 + 2, cur_col ) = - friction_disk[i].z();
// Effect on orientation of body j
{
const Vector3s ntilde1{ m_r1.cross( friction_disk[i] ) };
D.insert( 3 * ( m_idx1 + nbodies ) + 0, cur_col ) = - ntilde1.x();
D.insert( 3 * ( m_idx1 + nbodies ) + 1, cur_col ) = - ntilde1.y();
D.insert( 3 * ( m_idx1 + nbodies ) + 2, cur_col ) = - ntilde1.z();
}
}
}