RigidBody::RigidBody( const Vector2s& v, const scalar& omega, const VectorXs& vertices, const VectorXs& masses, const scalar& radius ) : m_M(-1.0) //, m_masses(masses) , m_I(-1.0) , m_vertices(vertices) , m_r(radius) , m_X(-1.0,-1.0) , m_theta(0.0) , m_V(v) , m_omega(omega) // theta == 0 => identity matrix , m_R(Matrix2s::Identity()) , m_F(0.0,0.0) , m_tau(0.0) { m_M = computeTotalMass(masses); m_X = computeCenterOfMass(vertices,masses); m_I = computeMomentOfInertia(vertices,masses); assert( (masses.array()>0.0).all() ); assert( m_M > 0.0 ); assert( m_I > 0.0 ); assert( m_vertices.size() >= 2 ); assert( m_r >= 0.0 ); // Translate the rigid body so the body space center of mass is the origin for( int i = 0; i < m_vertices.size()/2; ++i ) m_vertices.segment<2>(2*i) -= m_X; }
void ScriptingCallback::frictionCoefficientCallback( const std::vector<std::unique_ptr<Constraint>>& active_set, VectorXs& mu ) { if( name() == "" ) { return; } frictionCoefficient( active_set, mu ); assert( ( mu.array() >= 0.0 ).all() ); }
void ScriptingCallback::restitutionCoefficientCallback( const std::vector<std::unique_ptr<Constraint>>& active_set, VectorXs& cor ) { if( name() == "" ) { return; } restitutionCoefficient( active_set, cor ); assert( ( cor.array() >= 0.0 ).all() ); assert( ( cor.array() <= 1.0 ).all() ); }
void GRROperator::flow( const std::vector<std::unique_ptr<Constraint>>& cons, const SparseMatrixsc& M, const SparseMatrixsc& Minv, const VectorXs& q0, const VectorXs& v0, const VectorXs& v0F, const SparseMatrixsc& N, const SparseMatrixsc& Q, const VectorXs& nrel, const VectorXs& CoR, VectorXs& alpha ) { // Not intended for use with staggered projections assert( ( v0.array() == v0F.array() ).all() ); // Only works with one CoR assert( ( CoR.array() == CoR(0) ).all() ); if( ( CoR.array() != CoR(0) ).any() ) { std::cerr << "Error, GRR only supports a single coefficient of restitution. Exiting." << std::endl; std::exit( EXIT_FAILURE ); } // If the CoR is 0 or 1, don't waste time calling the other map if( CoR(0) == 0.0 ) { m_inelastic_operator->flow( cons, M, Minv, q0, v0, v0F, N, Q, nrel, CoR, alpha ); return; } else if( CoR(0) == 1.0 ) { m_elastic_operator->flow( cons, M, Minv, q0, v0, v0F, N, Q, nrel, CoR, alpha ); return; } // Create temporary storage VectorXs alpha_out{ alpha.size() }; alpha_out.setZero(); assert( m_inelastic_operator != nullptr ); m_inelastic_operator->flow( cons, M, Minv, q0, v0, v0F, N, Q, nrel, VectorXs::Zero( CoR.size() ), alpha_out ); //v1 = ( 1.0 - CoR ) * v_out; alpha = ( 1.0 - CoR(0) ) * alpha_out; alpha_out.setZero(); assert( m_elastic_operator != nullptr ); m_elastic_operator->flow( cons, M, Minv, q0, v0, v0F, N, Q, nrel, VectorXs::Ones( CoR.size() ), alpha_out ); //v1 += CoR * v_out; alpha += CoR(0) * alpha_out; }
void FrictionOperatorUtilities::projectOnFrictionDisc( const VectorXs& disc_bounds, VectorXs& beta ) { assert( beta.size() % 2 == 0 ); assert( beta.size() / 2 == disc_bounds.size() ); assert( ( disc_bounds.array() >= 0.0 ).all() ); for( int con_num = 0; con_num < disc_bounds.size(); ++con_num ) { const scalar dsc_nrm_sqrd{ beta.segment<2>( 2 * con_num ).squaredNorm() }; // If the squared norm of the friction impulse is greater than the \mu \alpha squared if( dsc_nrm_sqrd > disc_bounds( con_num ) * disc_bounds( con_num ) ) { // Normalize beta and rescale so its magnitude is \mu \alpha beta.segment<2>( 2 * con_num ) *= disc_bounds( con_num ) / sqrt( dsc_nrm_sqrd ); assert( fabs( beta.segment<2>( 2 * con_num ).norm() - disc_bounds( con_num ) ) <= 1.0e-6 ); } } }