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();
}
}
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;
}