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;
}
void StaticPlaneSphereConstraint::evalH( const VectorXs& q, const MatrixXXsc& basis, MatrixXXsc& H0, MatrixXXsc& H1 ) const
{
assert( H0.rows() == 3 );
assert( H0.cols() == 6 );
assert( H1.rows() == 3 );
assert( H1.cols() == 6 );
// Grab the contact normal
const Vector3s n{ basis.col( 0 ) };
// Grab the tangent basis
const Vector3s s{ basis.col( 1 ) };
const Vector3s t{ basis.col( 2 ) };
assert( MathUtilities::isRightHandedOrthoNormal( n, s, t, 1.0e-6 ) );
// Compute the displacement from the center of mass to the point of contact
assert( m_r >= 0.0 );
const Vector3s r_world{ - m_r * n };
H0.block<1,3>(0,0) = n;
H0.block<1,3>(0,3).setZero();
H0.block<1,3>(1,0) = s;
H0.block<1,3>(1,3) = r_world.cross( s );
H0.block<1,3>(2,0) = t;
H0.block<1,3>(2,3) = r_world.cross( t );
}
void FrictionUtilities::generateOrthogonalVectors( const Vector3s& n, std::vector<Vector3s>& vectors, const Vector3s& suggested_tangent )
{
if( vectors.empty() )
{
return;
}
assert( ( suggested_tangent.cross( n ) ).squaredNorm() != 0.0 );
// Make sure the first vector is orthogonal to n and unit length
vectors[0] = ( suggested_tangent - n.dot( suggested_tangent ) * n ).normalized();
assert( fabs( vectors[0].norm() - 1.0 ) <= 1.0e-10 );
assert( fabs( n.dot( vectors[0] ) ) <= 1.0e-10 );
// Generate the remaining vectors by rotating the first vector about n
const scalar dtheta{ 2.0 * MathDefines::PI<scalar>() / scalar( vectors.size() ) };
for( std::vector<Vector3s>::size_type i = 1; i < vectors.size(); ++i )
{
vectors[i] = Eigen::AngleAxis<scalar>{ i * dtheta, n } * vectors[0];
assert( fabs( vectors[i].norm() - 1.0 ) <= 1.0e-10 );
assert( fabs( n.dot( vectors[i] ) ) <= 1.0e-10 );
}
#ifndef NDEBUG
if( vectors.size() == 1 )
{
return;
}
// Check that the angle between each vector is the one we expect
for( std::vector<Vector3s>::size_type i = 0; i < vectors.size(); ++i )
{
assert( fabs( angleBetweenVectors( vectors[i], vectors[( i + 1 ) % vectors.size()] ) - dtheta ) < 1.0e-6 );
}
#endif
}
void TwoDSceneXMLParser::loadDragDampingForces( rapidxml::xml_node<>* node, TwoDScene& twodscene )
{
assert( node != NULL );
int forcenum = 0;
for( rapidxml::xml_node<>* nd = node->first_node("dragdamping"); nd; nd = nd->next_sibling("dragdamping") )
{
Vector3s constforce;
constforce.setConstant(std::numeric_limits<scalar>::signaling_NaN());
// Extract the linear damping coefficient
scalar b = std::numeric_limits<scalar>::signaling_NaN();
if( nd->first_attribute("b") )
{
std::string attribute(nd->first_attribute("b")->value());
if( !stringutils::extractFromString(attribute,b) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of b attribute for dragdamping " << forcenum << ". Value must be numeric. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse b attribute for dragdamping " << forcenum << ". Exiting." << std::endl;
exit(1);
}
//std::cout << "x: " << constforce.transpose() << std::endl;
twodscene.insertForce(new DragDampingForce(b));
++forcenum;
}
}
Vector3s StaticPlaneSphereConstraint::computePlaneCollisionPointVelocity( const VectorXs& q ) const
{
const Vector3s n{ m_plane.n() };
// Compute the collision point on the plane relative to x
const Vector3s plane_point{ ( q.segment<3>( 3 * m_sphere_idx ) - m_plane.x() ) - n.dot( q.segment<3>( 3 * m_sphere_idx ) - m_plane.x() ) * n };
return m_plane.v() + m_plane.omega().cross( plane_point );
}
void RigidBodySphere::computeMassAndInertia( const scalar& density, scalar& M, Vector3s& CM, Vector3s& I, Matrix33sr& R ) const
{
M = 4.0 * MathDefines::PI<scalar>() * m_r * m_r * m_r / 3.0;
I.setConstant( 2.0 * M * m_r * m_r / 5.0 );
M *= density;
I *= density;
CM.setZero();
R.setIdentity();
}
// 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 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 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 CollisionUtilities::computeBoxSphereActiveSet( const Vector3s& xb, const Matrix33sc& Rb, const Vector3s& wb, const Vector3s& xs, const scalar& rs, std::vector<Vector3s>& p, std::vector<Vector3s>& n )
{
// Rotation matrix should be orthonormal and orientation preserving
assert( ( Rb * Rb.transpose() - Matrix33sc::Identity() ).lpNorm<Eigen::Infinity>() <= 1.0e-6 );
assert( fabs( Rb.determinant() - 1.0 ) <= 1.0e-6 );
// All half-widths should be positive
assert( ( wb.array() > 0.0 ).all() );
Vector3s closest_point;
computeClosestPointToBox( xb, Rb, wb, xs, closest_point );
const scalar dist_squared{ ( closest_point - xs ).squaredNorm() };
// If the closest point is outside the sphere's radius, there is no collision
if( dist_squared > rs * rs )
{
return;
}
// If we are inside the box, throw an error
// TODO: Handle this case
if( dist_squared <= 1.0e-9 )
{
std::cerr << "Error, degenerate case in Box-Sphere collision detection. Implement degenerate CD code or decrease the time step! Exiting." << std::endl;
std::exit( EXIT_FAILURE );
}
// Compute the collision normal pointing from the sphere to the box
const Vector3s normal{ ( closest_point - xs ) / sqrt( dist_squared ) };
// Return the collision
p.push_back( closest_point );
n.push_back( normal );
}
scalar CollisionUtilities::closestPointPointSegment( const Vector3s& c, const Vector3s& a, const Vector3s& b, scalar& t )
{
const Vector3s ab{ b - a };
// Project c onto ab, computing parameterized position d(t) = a + t*(b – a)
t = (c - a).dot( ab ) / ab.dot( ab );
// If outside segment, clamp t (and therefore d) to the closest endpoint
if( t < 0.0 )
{
t = 0.0;
}
if( t > 1.0 )
{
t = 1.0;
}
// Compute projected position from the clamped t
const Vector3s& d{ a + t * ab };
return ( c - d ).squaredNorm();
}
StaticCylinder::StaticCylinder( const Vector3s& x, const Vector3s& axis, const scalar& radius )
: m_x( x )
, m_R( Quaternions::FromTwoVectors( Vector3s::UnitY(), axis.normalized() ) )
, m_v( Vector3s::Zero() )
, m_omega( Vector3s::Zero() )
, m_r( radius )
{
assert( fabs ( m_R.norm() - 1.0 ) < 1.0e-6 );
assert( m_r > 0.0 );
}
void VortexForce::addGradEToTotal( const VectorXs& x, const VectorXs& v, const VectorXs& m, VectorXs& gradE )
{
assert( x.size() == v.size() );
assert( x.size() == m.size() );
assert( x.size() == gradE.size() );
assert( x.size()%3 == 0 );
assert( m_particles.first >= 0 ); assert( m_particles.first < x.size()/3 );
assert( m_particles.second >= 0 ); assert( m_particles.second < x.size()/3 );
Vector3s rhat = x.segment<3>(3*m_particles.second)-x.segment<3>(3*m_particles.first);
scalar r = rhat.norm();
assert( r != 0.0 );
rhat /= r;
rhat *= m_kbs;
// Rotate rhat 90 degrees clockwise
scalar temp = rhat.x();
rhat.x() = -rhat.y();
rhat.y() = temp;
rhat -= v.segment<3>(3*m_particles.second)-v.segment<3>(3*m_particles.first);
rhat /= r*r;
rhat *= m_kvc;
gradE.segment<3>(3*m_particles.first) -= -rhat;
gradE.segment<3>(3*m_particles.second) += -rhat;
}
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 void diagonalizeInertiaTensor( const Matrix3s& I, Matrix3s& R0, Vector3s& I0 )
{
// Inertia tensor should by symmetric
assert( ( I - I.transpose() ).lpNorm<Eigen::Infinity>() <= 1.0e-6 );
// Inertia tensor should have positive determinant
assert( I.determinant() > 0.0 );
// Compute the eigenvectors and eigenvalues of the input matrix
const Eigen::SelfAdjointEigenSolver<Matrix3s> es{ I };
// Check for errors
if( es.info() == Eigen::NumericalIssue )
{
std::cerr << "Warning, failed to compute eigenvalues of inertia tensor due to Eigen::NumericalIssue" << std::endl;
}
else if( es.info() == Eigen::NoConvergence )
{
std::cerr << "Warning, failed to compute eigenvalues of inertia tensor due to Eigen::NoConvergence" << std::endl;
}
else if( es.info() == Eigen::InvalidInput )
{
std::cerr << "Warning, failed to compute eigenvalues of inertia tensor due to Eigen::InvalidInput" << std::endl;
}
assert( es.info() == Eigen::Success );
// Save the eigenvectors and eigenvalues
I0 = es.eigenvalues();
assert( ( I0.array() > 0.0 ).all() );
assert( I0.x() <= I0.y() );
assert( I0.y() <= I0.z() );
R0 = es.eigenvectors();
assert( fabs( fabs( R0.determinant() ) - 1.0 ) <= 1.0e-6 );
// Ensure that we have an orientation preserving transform
if( R0.determinant() < 0.0 )
{
R0.col( 0 ) *= -1.0;
}
}
void StapleStapleUtilities::computeStapleHalfPlaneActiveSet( const Vector3s& cm, const Matrix33sr& R, const RigidBodyStaple& staple,
const Vector3s& x0, const Vector3s& n, std::vector<int>& points )
{
// For each vertex of the staple
for( int i = 0; i < 4; ++i )
{
const Vector3s v{ R * staple.points()[ i ] + cm };
// Compute the distance from the vertex to the halfplane
const scalar d{ n.dot( v - x0 ) - staple.r() };
// If the distance is not positive, we have a collision!
if( d <= 0.0 )
{
points.emplace_back( i );
}
}
}
// TODO: This doesn't handle <0,0,0>
Vector3s FrictionUtilities::orthogonalVector( const Vector3s& n )
{
assert( fabs( n.norm() - 1.0 ) <= 1.0e-6 ); // TODO: Remove this
// Chose the most orthogonal direction among x, y, z
Vector3s orthog{ fabs(n.x()) <= fabs(n.y()) && fabs(n.x()) <= fabs(n.z()) ? Vector3s::UnitX() : fabs(n.y()) <= fabs(n.z()) ? Vector3s::UnitY() : Vector3s::UnitZ() };
assert( orthog.cross(n).squaredNorm() != 0.0 ); // New vector shouldn't be parallel to the input
// Project out any non-orthogonal component
orthog -= n.dot( orthog ) * n;
assert( orthog.norm() != 0.0 );
return orthog.normalized();
}
bool MathUtilities::isRightHandedOrthoNormal( const Vector3s& a, const Vector3s& b, const Vector3s& c, const scalar& tol )
{
// All basis vectors should be unit
if( fabs( a.norm() - 1.0 ) > tol ) { return false; }
if( fabs( b.norm() - 1.0 ) > tol ) { return false; }
if( fabs( c.norm() - 1.0 ) > tol ) { return false; }
// All basis vectors should be mutually orthogonal
if( fabs( a.dot( b ) ) > tol ) { return false; }
if( fabs( a.dot( c ) ) > tol ) { return false; }
if( fabs( b.dot( c ) ) > tol ) { return false; }
// Coordinate system should be right handed
if( ( a.cross( b ) - c ).lpNorm<Eigen::Infinity>() > tol ) { return false; }
return true;
}
static PyObject* setStaticPlaneVelocity( PyObject* self, PyObject* args )
{
using std::is_same;
static_assert( is_same<scalar,double>::value || is_same<scalar,float>::value, "Error, scalar type must be double or float for Python interface." );
unsigned plane_idx;
Vector3s velocity;
assert( args != nullptr );
if( !PyArg_ParseTuple( args, is_same<scalar,double>::value ? "Iddd" : "Ifff", &plane_idx, &velocity.x(), &velocity.y(), &velocity.z() ) )
{
PyErr_Print();
std::cerr << "Failed to read parameters for setStaticPlaneVelocity, parameters are: plane_idx, vx, vy, vz. Exiting." << std::endl;
std::exit( EXIT_FAILURE );
}
assert( s_sim_state != nullptr );
if( plane_idx > s_sim_state->staticPlanes().size() )
{
std::cerr << "Invalid plane_idx parameter of " << plane_idx << " in setStaticPlaneVelocity, plane_idx must be less than " << s_sim_state->staticPlanes().size() << ". Exiting." << std::endl;
std::exit( EXIT_FAILURE );
}
s_sim_state->staticPlane(plane_idx).v() = velocity;
return Py_BuildValue( "" );
}
static PyObject* setStaticCylinderAngularVelocity( PyObject* self, PyObject* args )
{
using std::is_same;
static_assert( is_same<scalar,double>::value || is_same<scalar,float>::value, "Error, scalar type must be double or float for Python interface." );
unsigned cylinder_idx;
Vector3s omega;
assert( args != nullptr );
if( !PyArg_ParseTuple( args, is_same<scalar,double>::value ? "Iddd" : "Ifff", &cylinder_idx, &omega.x(), &omega.y(), &omega.z() ) )
{
PyErr_Print();
std::cerr << "Failed to read parameters for setStaticCylinderAngularVelocity, parameters are: cylinder_idx, omegax, omegay, omegaz. Exiting." << std::endl;
std::exit( EXIT_FAILURE );
}
assert( s_sim_state != nullptr );
if( cylinder_idx > s_sim_state->staticCylinders().size() )
{
std::cerr << "Invalid cylinder_idx parameter of " << cylinder_idx << " in setStaticCylinderAngularVelocity, cylinder_idx must be less than " << s_sim_state->staticCylinders().size() << ". Exiting." << std::endl;
std::exit( EXIT_FAILURE );
}
s_sim_state->staticCylinder(cylinder_idx).omega() = omega;
return Py_BuildValue( "" );
}
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 RigidBodySphere::computeAABB( const Vector3s& cm, const Matrix33sr& R, Array3s& min, Array3s& max ) const
{
min = cm.array() - m_r;
max = cm.array() + m_r;
}
StaticPlane::StaticPlane( const Vector3s& x, const Vector3s& n )
: m_x( x )
, m_n( n.normalized() )
, m_v( Vector3s::Zero() )
, m_omega( Vector3s::Zero() )
{}
void TwoDSceneXMLParser::loadParticles( rapidxml::xml_node<>* node, TwoDScene& twodscene )
{
assert( node != NULL );
// Count the number of particles
int numparticles = 0;
for( rapidxml::xml_node<>* nd = node->first_node("particle"); nd; nd = nd->next_sibling("particle") ) ++numparticles;
twodscene.resizeSystem(numparticles);
//std::cout << "Num particles " << numparticles << std::endl;
std::vector<std::string>& tags = twodscene.getParticleTags();
int particle = 0;
for( rapidxml::xml_node<>* nd = node->first_node("particle"); nd; nd = nd->next_sibling("particle") )
{
// Extract the particle's initial position
Vector3s pos;
if( nd->first_attribute("px") )
{
std::string attribute(nd->first_attribute("px")->value());
if( !stringutils::extractFromString(attribute,pos.x()) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of px attribute for particle " << particle << ". Value must be numeric. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse px attribute for particle " << particle << ". Exiting." << std::endl;
exit(1);
}
if( nd->first_attribute("py") )
{
std::string attribute(nd->first_attribute("py")->value());
if( !stringutils::extractFromString(attribute,pos.y()) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of py attribute for particle " << particle << ". Value must be numeric. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse py attribute for particle " << particle << ". Exiting." << std::endl;
exit(1);
}
if( nd->first_attribute("pz") )
{
std::string attribute(nd->first_attribute("pz")->value());
if( !stringutils::extractFromString(attribute,pos.z()) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of pz attribute for particle " << particle << ". Value must be numeric. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse pz attribute for particle " << particle << ". Exiting." << std::endl;
exit(1);
}
twodscene.setPosition( particle, pos );
// Extract the particle's initial velocity
Vector3s vel;
if( nd->first_attribute("vx") )
{
std::string attribute(nd->first_attribute("vx")->value());
if( !stringutils::extractFromString(attribute,vel.x()) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of vx attribute for particle " << particle << ". Value must be numeric. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse vx attribute for particle " << particle << ". Exiting." << std::endl;
exit(1);
}
if( nd->first_attribute("vy") )
{
std::string attribute(nd->first_attribute("vy")->value());
if( !stringutils::extractFromString(attribute,vel.y()) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of vy attribute for particle " << particle << ". Value must be numeric. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse vy attribute for particle " << particle << ". Exiting." << std::endl;
exit(1);
}
if( nd->first_attribute("vz") )
{
std::string attribute(nd->first_attribute("vz")->value());
if( !stringutils::extractFromString(attribute,vel.z()) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of vz attribute for particle " << particle << ". Value must be numeric. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse vz attribute for particle " << particle << ". Exiting." << std::endl;
exit(1);
}
twodscene.setVelocity( particle, vel );
// Extract the particle's mass
scalar mass = std::numeric_limits<scalar>::signaling_NaN();
if( nd->first_attribute("m") )
{
std::string attribute(nd->first_attribute("m")->value());
if( !stringutils::extractFromString(attribute,mass) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of m attribute for particle " << particle << ". Value must be numeric. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse m attribute for particle " << particle << ". Exiting." << std::endl;
exit(1);
}
twodscene.setMass( particle, mass );
// Determine if the particle is fixed
bool fixed;
if( nd->first_attribute("fixed") )
{
std::string attribute(nd->first_attribute("fixed")->value());
if( !stringutils::extractFromString(attribute,fixed) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse value of fixed attribute for particle " << particle << ". Value must be boolean. Exiting." << std::endl;
exit(1);
}
}
else
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse fixed attribute for particle " << particle << ". Exiting." << std::endl;
exit(1);
}
twodscene.setFixed( particle, fixed );
// Extract the particle's radius, if present
scalar radius = 0.1;
if( nd->first_attribute("radius") )
{
std::string attribute(nd->first_attribute("radius")->value());
if( !stringutils::extractFromString(attribute,radius) )
{
std::cerr << "\033[31;1mERROR IN XMLSCENEPARSER:\033[m Failed to parse radius attribute for particle " << particle << ". Value must be scalar. Exiting." << std::endl;
exit(1);
}
}
twodscene.setRadius( particle, radius );
// Extract the particle's tag, if present
if( nd->first_attribute("tag") )
{
std::string tag(nd->first_attribute("tag")->value());
tags[particle] = tag;
}
//std::cout << "Particle: " << particle << " x: " << pos.transpose() << " v: " << vel.transpose() << " m: " << mass << " fixed: " << fixed << std::endl;
//std::cout << tags[particle] << std::endl;
++particle;
}
}
bool XMLExporter::saveToXMLFile( const std::string& file_name, const RigidBody3DState& state )
{
std::ofstream xml_file( file_name );
if( !xml_file.is_open() )
{
std::cerr << "Error, failed to open file " << file_name << " for output." << std::endl;
return false;
}
xml_file << "<rigidbody3d_scene>" << std::endl;
// Save the geometry out
for( const std::unique_ptr<RigidBodyGeometry>& geo : state.geometry() )
{
switch( geo->getType() )
{
case RigidBodyGeometryType::SPHERE:
{
const RigidBodySphere& sphere{ static_cast<RigidBodySphere&>( *geo.get() ) };
xml_file << " <geometry type=\"sphere\" r=\"" << sphere.r() << "\"/>" << std::endl;
break;
}
case RigidBodyGeometryType::BOX:
{
std::cerr << "Code up box geometry xml output." << std::endl;
std::exit( EXIT_FAILURE );
}
case RigidBodyGeometryType::STAPLE:
{
std::cerr << "Code up staple geometry xml output." << std::endl;
std::exit( EXIT_FAILURE );
}
case RigidBodyGeometryType::TRIANGLE_MESH:
{
const RigidBodyTriangleMesh& tri_mesh{ static_cast<RigidBodyTriangleMesh&>( *geo.get() ) };
xml_file << " <geometry type=\"mesh\" filename=\"" << tri_mesh.inputFileName() << "\"/>" << std::endl;
break;
}
}
}
// Save the bodies out
for( unsigned bdy_idx = 0; bdy_idx < state.nbodies(); bdy_idx++ )
{
const Vector3s x{ state.q().segment<3>( 3 * bdy_idx ) };
const Matrix33sr R{ Eigen::Map<const Matrix33sr>{ state.q().segment<9>( 3 * state.nbodies() + 9 * bdy_idx ).data() } };
using std::fabs;
assert( fabs( R.determinant() - 1.0 ) <= 1.0e-9 );
assert( ( R.transpose() * R - Matrix33sr::Identity() ).lpNorm<Eigen::Infinity>() <= 1.0e-9 );
const Vector3s R_aa{ matrixToAngleAxis( R ) };
const Vector3s v{ state.v().segment<3>( 3 * bdy_idx ) };
const Vector3s omega{ state.v().segment<3>( 3 * state.nbodies() + 3 * bdy_idx ) };
const scalar density{ state.getTotalMass(bdy_idx) / state.getGeometryOfBody(bdy_idx).volume() };
const bool is_fixed{ state.isKinematicallyScripted(bdy_idx) };
const unsigned geo_idx{ state.getGeometryIndexOfBody(bdy_idx) };
xml_file << " <rigid_body_with_density x=\"" << x.x() << " " << x.y() << " " << x.z() << "\" R=\"" << R_aa.x() << " " << R_aa.y() << " " << R_aa.z() << "\" v=\"" << v.x() << " " << v.y() << " " << v.z() << "\" omega=\"" << omega.x() << " " << omega.y() << " " << omega.z() << "\" rho=\"" << density << "\" fixed=\"" << is_fixed << "\" geo_idx=\"" << geo_idx << "\"/>" << std::endl;
}
xml_file << "</rigidbody3d_scene>" << std::endl;
return true;
}
scalar CollisionUtilities::closestPointSegmentSegment( const Vector3s& p1, const Vector3s& q1, const Vector3s& p2, const Vector3s& q2, scalar& s, scalar& t, Vector3s& c1, Vector3s& c2 )
{
const scalar DIST_EPS{ 1.0e-8 };
const Vector3s d1{ q1 - p1 }; // Direction vector of segment S1
const Vector3s d2{ q2 - p2}; // Direction vector of segment S2
const Vector3s r{ p1 - p2 };
const scalar a{ d1.dot( d1 ) }; // Squared length of segment S1, always nonnegative
const scalar e{ d2.dot( d2 ) }; // Squared length of segment S2, always nonnegative
const scalar f{ d2.dot( r ) };
// Check if either or both segments degenerate into points
if( a <= DIST_EPS && e <= DIST_EPS )
{
// Both segments degenerate into points
s = t = 0.0;
c1 = p1;
c2 = p2;
return ( c1 - c2 ).dot( c1 - c2 );
}
if( a <= DIST_EPS )
{
// First segment degenerates into a point
s = 0.0;
t = f / e; // s = 0 => t = (b*s + f) / e = f / e
t = clamp( t, 0.0, 1.0 );
}
else
{
const scalar c{ d1.dot( r ) };
if( e <= DIST_EPS )
{
// Second segment degenerates into a point
t = 0.0;
s = clamp( -c / a, 0.0, 1.0 ); // t = 0 => s = (b*t - c) / a = -c / a
}
else
{
// The general nondegenerate case starts here
const scalar b{ d1.dot( d2 ) };
const scalar denom{ a * e - b * b }; // Always nonnegative
// If segments not parallel, compute closest point on L1 to L2, and
// clamp to segment S1. Else pick arbitrary s (here 0)
if( denom != 0.0 )
{
s = clamp( ( b * f - c * e ) / denom, 0.0, 1.0 );
}
else
{
s = 0.0;
}
// Compute point on L2 closest to S1(s) using
// t = Dot((P1+D1*s)-P2,D2) / Dot(D2,D2) = (b*s + f) / e
t = ( b * s + f ) / e;
// If t in [0,1] done. Else clamp t, recompute s for the new value
// of t using s = Dot((P2+D2*t)-P1,D1) / Dot(D1,D1)= (t*b - c) / a
// and clamp s to [0, 1]
if( t < 0.0 )
{
t = 0.0;
s = clamp( -c / a, 0.0, 1.0 );
}
else if( t > 1.0 )
{
t = 1.0;
s = clamp( (b - c) / a, 0.0, 1.0 );
}
}
}
c1 = p1 + d1 * s;
c2 = p2 + d2 * t;
return ( c1 - c2 ).dot( c1 - c2 );
}
// Unsigned angle
scalar angleBetweenVectors( const Vector3s& v0, const Vector3s& v1 )
{
const scalar s = v0.cross( v1 ).norm();
const scalar c = v0.dot( v1 );
return atan2( s, c );
}
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();
}
}
}
bool StaticPlaneSphereConstraint::isActive( const Vector3s& x_plane, const Vector3s& n_plane, const Vector3s& x_sphere, const scalar& r )
{
assert( fabs( n_plane.norm() - 1.0 ) <= 1.0e-6 );
return n_plane.dot( x_sphere - x_plane ) <= r;
}
