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;
}
}
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 Ball2DState::setMass( const VectorXs& m )
{
m_M = createM( m );
m_Minv = createMinv( m );
#ifndef NDEBUG
const SparseMatrixsc should_be_id{ m_M * m_Minv };
const Eigen::Map<const ArrayXs> should_be_id_data{ should_be_id.valuePtr(), should_be_id.nonZeros() };
assert( ( ( should_be_id_data - 1.0 ).abs() <= 1.0e-6 ).all() );
#endif
}
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;
}
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();
}
