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(); }
// 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 ); } }
static void buildLinearFrictionBasis( const VectorXs& q, const VectorXs& v, const int nsamples, const std::vector<std::unique_ptr<Constraint>>& K, SparseMatrixsc& D, VectorXs& drel ) { assert( D.cols() % nsamples == 0 ); const unsigned ncons{ static_cast<unsigned>( D.cols() ) / nsamples }; std::vector<std::unique_ptr<Constraint>>::const_iterator itr{ K.begin() }; for( unsigned con_idx = 0; con_idx < ncons; ++con_idx ) { (*itr)->computeGeneralizedFrictionDisk( q, v, con_idx * nsamples, nsamples, D, drel ); ++itr; } assert( itr == K.end() ); }
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 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(); } }
scalar HertzianPenaltyForce::computePotential( const VectorXs& q, const SparseMatrixsc& M, const VectorXs& r ) const { assert( q.size() % 2 == 0 ); assert( q.size() == M.rows() ); assert( q.size() == M.cols() ); assert( r.size() == q.size() / 2 ); scalar U{ 0.0 }; // For each ball for( unsigned ball0 = 0; ball0 < r.size(); ++ball0 ) { // For each subsequent ball for( unsigned ball1 = ball0 + 1; ball1 < r.size(); ++ball1 ) { // Compute the total radius const scalar total_radius{ r(ball0) + r(ball1) }; // Compute a vector pointing from ball0 to ball1 const Vector2s n{ q.segment<2>( 2 * ball1 ) - q.segment<2>( 2 * ball0 ) }; // If the squared distance is greater or equal to the sum of the radii squared, no force if( n.squaredNorm() > total_radius * total_radius ) { continue; } // Compute the penetration depth const scalar delta{ n.norm() - total_radius }; assert( delta < 0.0 ); // U = 0.5 * k * pen_depth^(5/2) U += 0.5 * m_k * std::pow( -delta, scalar( 2.5 ) ); } } return U; }
void MathUtilities::serialize( const SparseMatrixsc& A, std::ostream& stm ) { assert( stm.good() ); VectorXi col_ptr; VectorXi row_ind; VectorXs val; MathUtilities::extractDataCCS( A, col_ptr, row_ind, val ); assert( col_ptr.size() == A.cols() + 1 ); assert( row_ind.size() == A.nonZeros() ); assert( val.size() == A.nonZeros() ); // Size of col_ptr == A.cols() + 1 Utilities::serializeBuiltInType( A.rows(), stm ); Utilities::serializeBuiltInType( A.cols(), stm ); stm.write( reinterpret_cast<char*>( col_ptr.data() ), col_ptr.size() * sizeof(int) ); // Size of row_ind == size of val == A.nonZeros() Utilities::serializeBuiltInType( A.nonZeros(), stm ); stm.write( reinterpret_cast<char*>( row_ind.data() ), row_ind.size() * sizeof(int) ); stm.write( reinterpret_cast<char*>( val.data() ), val.size() * sizeof(scalar) ); }
static void reserveSpaceInBasisMatrix( const int nsamples, const std::vector<std::unique_ptr<Constraint>>& K, SparseMatrixsc& D ) { assert( D.cols() % nsamples == 0 ); const int ncons{ D.cols() / nsamples }; VectorXi column_nonzeros{ D.cols() }; std::vector<std::unique_ptr<Constraint>>::const_iterator itr{ K.begin() }; for( int con_idx = 0; con_idx < ncons; ++con_idx ) { for( int smpl_num = 0; smpl_num < nsamples; ++smpl_num ) { column_nonzeros( con_idx * nsamples + smpl_num ) = (*itr)->frictionStencilSize(); } ++itr; } assert( itr == K.end() ); D.reserve( column_nonzeros ); }
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() ); }
void Ball2DGravityForce::computeForce( const VectorXs& q, const VectorXs& v, const SparseMatrixsc& M, const VectorXs& r, VectorXs& result ) const { assert( q.size() % 2 == 0 ); assert( q.size() == v.size() ); assert( q.size() == M.rows() ); assert( q.size() == M.cols() ); assert( q.size() == result.size() ); for( int i = 0; i < q.size(); i += 2 ) { assert( M.valuePtr()[ i ] == M.valuePtr()[ i + 1 ] ); assert( M.valuePtr()[ i ] > 0.0 ); result.segment<2>( i ) += M.valuePtr()[ i ] * m_g; } }
// 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 }
scalar Ball2DGravityForce::computePotential( const VectorXs& q, const SparseMatrixsc& M, const VectorXs& r ) const { assert( q.size() % 2 == 0 ); assert( q.size() == M.rows() ); assert( q.size() == M.cols() ); scalar U{ 0.0 }; for( int i = 0; i < q.size(); i += 2 ) { assert( M.valuePtr()[ i ] == M.valuePtr()[ i + 1 ] ); assert( M.valuePtr()[ i ] > 0.0 ); U += - M.valuePtr()[ i ] * m_g.dot( q.segment<2>( i ) ); } return U; }
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 FrictionOperatorUtilities::formGeneralizedFrictionBasis( const VectorXs& q0, const VectorXs& v0, const std::vector<std::unique_ptr<Constraint>>& K, const int num_samples, SparseMatrixsc& D, VectorXs& drel ) { assert( num_samples > 0 ); assert( D.rows() == v0.size() ); assert( num_samples * int( K.size() ) == D.cols() ); // Reserve space for entries reserveSpaceInBasisMatrix( num_samples, K, D ); // Build the matrix buildLinearFrictionBasis( q0, v0, num_samples, K, D, drel ); D.makeCompressed(); }
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 MathUtilities::extractLowerTriangularMatrix( const SparseMatrixsc& A, SparseMatrixsc& B ) { std::vector< Eigen::Triplet<scalar> > triplets; for( int col = 0; col < A.outerSize(); ++col ) { for( SparseMatrixsc::InnerIterator it( A, col ); it; ++it ) { if( col > it.row() ) { continue; } triplets.push_back( Eigen::Triplet<scalar>( it.row(), col, it.value() ) ); } } B.resize( A.rows(), A.cols() ); B.setFromTriplets( triplets.begin(), triplets.end() ); B.makeCompressed(); }
void NearEarthGravityForce::computeForce( const VectorXs& q, const VectorXs& v, const SparseMatrixsc& M, VectorXs& result ) const { assert( q.size() % 12 == 0 ); assert( v.size() == q.size() / 2 ); assert( M.rows() == M.cols() ); assert( M.nonZeros() == q.size() ); const unsigned nbodies{ static_cast<unsigned>( q.size() / 12 ) }; const Eigen::Map<const VectorXs> masses{ M.valuePtr(), 3 * nbodies }; for( unsigned i = 0; i < nbodies; ++i ) { assert( masses( 3 * i + 0 ) == masses( 3 * i + 1 ) ); assert( masses( 3 * i + 1 ) == masses( 3 * i + 2 ) ); result.segment<3>( 3 * i ) += masses( 3 * i ) * m_g; } }
scalar NearEarthGravityForce::computePotential( const VectorXs& q, const SparseMatrixsc& M ) const { assert( q.size() % 12 == 0 ); assert( M.rows() == M.cols() ); assert( M.nonZeros() == q.size() ); const unsigned nbodies{ static_cast<unsigned>( q.size() / 12 ) }; const Eigen::Map<const VectorXs> masses{ M.valuePtr(), 3 * nbodies }; scalar U = 0.0; for( unsigned i = 0; i < nbodies; ++i ) { assert( masses( 3 * i + 0 ) == masses( 3 * i + 1 ) ); assert( masses( 3 * i + 1 ) == masses( 3 * i + 2 ) ); U += - masses( 3 * i ) * m_g.dot( q.segment<3>( 3 * i ) ); } return U; }
// TODO: Despecialize from smooth void FrictionOperator::formGeneralizedSmoothFrictionBasis( const unsigned ndofs, const unsigned ncons, const VectorXs& q, const std::vector<std::unique_ptr<Constraint>>& K, const MatrixXXsc& bases, SparseMatrixsc& D ) { assert( ncons == K.size() ); const unsigned nambientdims{ static_cast<unsigned>( bases.rows() ) }; const unsigned nsamples{ nambientdims - 1 }; D.resize( ndofs, nsamples * ncons ); auto itr = K.cbegin(); { VectorXi column_nonzeros( D.cols() ); for( unsigned collision_number = 0; collision_number < ncons; ++collision_number ) { for( unsigned sample_number = 0; sample_number < nsamples; ++sample_number ) { assert( nsamples * collision_number + sample_number < column_nonzeros.size() ); column_nonzeros( nsamples * collision_number + sample_number ) = (*itr)->frictionStencilSize(); } ++itr; } assert( ( column_nonzeros.array() > 0 ).all() ); assert( itr == K.cend() ); D.reserve( column_nonzeros ); } itr = K.cbegin(); for( unsigned collision_number = 0; collision_number < ncons; ++collision_number ) { for( unsigned sample_number = 0; sample_number < nsamples; ++sample_number ) { const unsigned current_column{ nsamples * collision_number + sample_number }; const VectorXs current_sample{ bases.col( nambientdims * collision_number + sample_number + 1 ) }; assert( fabs( current_sample.dot( bases.col( nambientdims * collision_number ) ) ) <= 1.0e-6 ); (*itr)->computeGeneralizedFrictionGivenTangentSample( q, current_sample, current_column, D ); } ++itr; } assert( itr == K.cend() ); D.prune( []( const Eigen::Index& row, const Eigen::Index& col, const scalar& value ) { return value != 0.0; } ); assert( D.innerNonZeroPtr() == nullptr ); }
void StaticPlaneSphereConstraint::resolveImpact( const scalar& CoR, const SparseMatrixsc& M, const scalar& ndotv, VectorXs& vout, scalar& alpha ) const { assert( CoR >= 0.0 ); assert( CoR <= 1.0 ); assert( ndotv < 0.0 ); assert( vout.size() % 3 == 0 ); assert( M.rows() == M.cols() ); assert( M.nonZeros() == 2 * vout.size() ); assert( 3 * m_sphere_idx + 2 < vout.size() ); const Eigen::Map<const VectorXs> m{ M.valuePtr(), vout.size() }; assert( m( 3 * m_sphere_idx ) == m( 3 * m_sphere_idx + 1 ) ); assert( m( 3 * m_sphere_idx ) == m( 3 * m_sphere_idx + 2 ) ); const scalar msphere{ m( 3 * m_sphere_idx ) }; // Compute the impulse alpha = - ( 1.0 + CoR ) * ndotv * msphere; assert( alpha >= 0.0 ); vout.segment<3>( 3 * m_sphere_idx ) += - ( 1.0 + CoR ) * ndotv * m_plane.n(); }
void LCPOperatorQL::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 ) { // Q in 1/2 \alpha^T Q \alpha assert( Q.rows() == Q.cols() ); MatrixXXsc Qdense = Q; // Linear term in the objective VectorXs Adense; ImpactOperatorUtilities::computeLCPQPLinearTerm( N, nrel, CoR, v0, v0F, Adense ); // Solve the QP assert( Qdense.rows() == Adense.size() ); assert( Adense.size() == alpha.size() ); const int status = solveQP( m_tol, Qdense, Adense, alpha ); // Check for problems if( 0 != status ) { std::cerr << "Warning, failed to solve QP in LCPOperatorQL::flow: " << QLUtilities::QLReturnStatusToString(status) << std::endl; } // TODO: Sanity check the solution here }
void MathUtilities::extractDataCCS( const SparseMatrixsc& A, VectorXi& col_ptr, VectorXi& row_ind, VectorXs& val ) { col_ptr.resize( A.cols() + 1 ); row_ind.resize( A.nonZeros() ); val.resize( A.nonZeros() ); col_ptr(0) = 0; for( int col = 0; col < A.outerSize(); ++col ) { col_ptr(col+1) = col_ptr(col); for( SparseMatrixsc::InnerIterator it(A,col); it; ++it ) { const int row{ it.row() }; val(col_ptr(col+1)) = it.value(); row_ind(col_ptr(col+1)) = row; ++col_ptr(col+1); } } assert( col_ptr( col_ptr.size() - 1 ) == row_ind.size() ); }
void HertzianPenaltyForce::computeForce( const VectorXs& q, const VectorXs& v, const SparseMatrixsc& M, const VectorXs& r, VectorXs& result ) const { assert( q.size() % 2 == 0 ); assert( q.size() == v.size() ); assert( q.size() == M.rows() ); assert( q.size() == M.cols() ); assert( r.size() == q.size() / 2 ); assert( q.size() == result.size() ); // For each ball for( unsigned ball0 = 0; ball0 < r.size(); ++ball0 ) { // For each subsequent ball for( unsigned ball1 = ball0 + 1; ball1 < r.size(); ++ball1 ) { // Compute the total radius const scalar total_radius{ r(ball0) + r(ball1) }; // Compute a vector pointing from ball0 to ball1 Vector2s n{ q.segment<2>( 2 * ball1 ) - q.segment<2>( 2 * ball0 ) }; // Compute the squared length of the vector scalar d{ n.squaredNorm() }; // If the squared distance is greater or equal to the sum of the radii squared, no force if( d > total_radius * total_radius ) { continue; } // Normalize the vector between the balls d = sqrt( d ); assert( d != 0.0 ); n /= d; assert( fabs( n.norm() - 1.0 ) <= 1.0e-6 ); // Compute the penetration depth d -= total_radius; assert( d < 0.0 ); // F = 5 * k * pen_depth^(3/2) * n const Vector2s F{ ( 5.0 / 4.0 ) * m_k * std::pow( -d, scalar( 1.5 ) ) * n }; result.segment<2>( 2 * ball1 ) += F; result.segment<2>( 2 * ball0 ) -= F; } } }
bool MathUtilities::isSquare( const SparseMatrixsc& matrix ) { return matrix.rows() == matrix.cols(); }
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(); } } }
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; } }