// 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 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();
}
}
void TeleportedCircleCircleConstraint::evalgradg( const VectorXs& q, const int col, SparseMatrixsc& G, const FlowableSystem& fsys ) const
{
assert( col >= 0 ); assert( col < G.cols() );
// MUST BE ADDED GOING DOWN THE COLUMN. DO NOT TOUCH ANOTHER COLUMN.
assert( m_idx0 < m_idx1 );
assert( 3 * m_idx0 + 1 < unsigned( G.rows() ) );
G.insert( 3 * m_idx0 + 0, col ) = m_n.x();
G.insert( 3 * m_idx0 + 1, col ) = m_n.y();
assert( 3 * m_idx1 + 1 < unsigned( G.rows() ) );
G.insert( 3 * m_idx1 + 0, col ) = - m_n.x();
G.insert( 3 * m_idx1 + 1, col ) = - m_n.y();
}
void StaticPlaneSphereConstraint::evalgradg( const VectorXs& q, const int col, SparseMatrixsc& G, const FlowableSystem& fsys ) const
{
assert( col >= 0 );
assert( col < G.cols() );
assert( 3 * m_sphere_idx + 2 < unsigned( G.rows() ) );
const Vector3s n{ m_plane.n() };
assert( fabs( n.norm() - 1.0 ) <= 1.0e-6 );
// MUST BE ADDED GOING DOWN THE COLUMN. DO NOT TOUCH ANOTHER COLUMN.
G.insert( 3 * m_sphere_idx + 0, col ) = n.x();
G.insert( 3 * m_sphere_idx + 1, col ) = n.y();
G.insert( 3 * m_sphere_idx + 2, col ) = n.z();
}
void BodyBodyConstraint::evalgradg( const VectorXs& q, const int col, SparseMatrixsc& G, const FlowableSystem& fsys ) const
{
assert( q.size() % 12 == 0 );
assert( col >= 0 );
assert( col < G.cols() );
const unsigned nbodies{ static_cast<unsigned>( q.size() / 12 ) };
// MUST BE ADDED GOING DOWN THE COLUMN. DO NOT TOUCH ANOTHER COLUMN.
{
assert( 3 * nbodies + 3 * m_idx0 + 2 < unsigned( G.rows() ) );
G.insert( 3 * m_idx0 + 0, col ) = m_n.x();
G.insert( 3 * m_idx0 + 1, col ) = m_n.y();
G.insert( 3 * m_idx0 + 2, col ) = m_n.z();
const Vector3s ntilde_0{ m_r0.cross( m_n ) };
G.insert( 3 * ( m_idx0 + nbodies ) + 0, col ) = ntilde_0.x();
G.insert( 3 * ( m_idx0 + nbodies ) + 1, col ) = ntilde_0.y();
G.insert( 3 * ( m_idx0 + nbodies ) + 2, col ) = ntilde_0.z();
}
{
assert( 3 * nbodies + 3 * m_idx1 + 2 < unsigned( G.rows() ) );
G.insert( 3 * m_idx1 + 0, col ) = - m_n.x();
G.insert( 3 * m_idx1 + 1, col ) = - m_n.y();
G.insert( 3 * m_idx1 + 2, col ) = - m_n.z();
const Vector3s ntilde_1{ m_r1.cross( m_n ) };
G.insert( 3 * ( m_idx1 + nbodies ) + 0, col ) = - ntilde_1.x();
G.insert( 3 * ( m_idx1 + nbodies ) + 1, col ) = - ntilde_1.y();
G.insert( 3 * ( m_idx1 + nbodies ) + 2, col ) = - ntilde_1.z();
}
}
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 MathUtilities::createDiagonalMatrix( const scalar& c, SparseMatrixsc& D )
{
assert( D.rows() == D.cols() );
D.reserve( VectorXi::Constant( D.cols(), 1 ) );
for( int i = 0; i < D.cols(); ++i ) { D.insert(i,i) = c; }
D.makeCompressed();
}
void FrictionOperatorUtilities::formLinearFrictionDiskConstraint( const int num_samples, SparseMatrixsc& E )
{
{
const VectorXi column_nonzeros{ VectorXi::Constant( E.cols(), num_samples ) };
E.reserve( column_nonzeros );
}
// For each column
for( int col = 0; col < E.cols(); ++col )
{
for( int samplenum = 0; samplenum < num_samples; ++samplenum )
{
// Note the negative for QL
E.insert( num_samples * col + samplenum, col ) = 1.0;
}
}
E.makeCompressed();
assert( E.nonZeros() == E.cols() * num_samples );
assert( E.sum() == E.nonZeros() );
}
// TODO: Pull the outerIndexPtr arithmetic into a helper function
void MathUtilities::extractColumns( const SparseMatrixsc& A0, const std::vector<unsigned>& cols, SparseMatrixsc& A1 )
{
const unsigned ncols_to_extract{ static_cast<unsigned>( cols.size() ) };
assert( ncols_to_extract <= static_cast<unsigned>( A0.cols() ) );
#ifndef NDEBUG
for( unsigned i = 0; i < ncols_to_extract; ++i )
{
assert( cols[i] < unsigned( A0.cols() ) );
}
#endif
// Compute the number of nonzeros in each column of the new matrix
VectorXi column_nonzeros{ ncols_to_extract };
for( unsigned i = 0; i < ncols_to_extract; ++i )
{
column_nonzeros( i ) = A0.outerIndexPtr()[cols[i]+1] - A0.outerIndexPtr()[cols[i]];
}
// Resize A1 and reserve space
A1.resize( A0.rows(), ncols_to_extract );
A1.reserve( column_nonzeros );
// Copy the data over, column by column
for( unsigned cur_col = 0; cur_col < ncols_to_extract; ++cur_col )
{
for( SparseMatrixsc::InnerIterator it( A0, cols[ cur_col ] ); it; ++it )
{
A1.insert( it.row(), cur_col ) = it.value();
}
}
A1.makeCompressed();
#ifndef NDEBUG
for( int i = 0 ; i < A1.cols(); ++i )
{
assert( ( A1.outerIndexPtr()[i+1] - A1.outerIndexPtr()[i] ) == column_nonzeros( i ) );
}
#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;
}
}
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();
}
}
}
