Exemplo n.º 1
1
void DragDampingForce::addHessVToTotal( const VectorXs& x, const VectorXs& v, const VectorXs& m, MatrixXs& hessE )
{
  assert( x.size() == v.size() );
  assert( x.size() == m.size() );
  assert( x.size() == hessE.rows() );
  assert( x.size() == hessE.cols() );
  assert( x.size()%2 == 0 );
  
  // Compute the force Jacboian here!
   hessE.diagonal().array() += m_b;
}
Exemplo n.º 2
0
void RigidBody2DSim::computeBodyBodyActiveSetSpatialGrid( const VectorXs& q0, const VectorXs& q1, const VectorXs& v, std::vector<std::unique_ptr<Constraint>>& active_set ) const
{
  assert( q0.size() % 3 == 0 ); assert( q0.size() == q1.size() );

  const unsigned nbodies{ static_cast<unsigned>( q0.size() / 3 ) };

  // Candidate bodies that might overlap
  std::set<std::pair<unsigned,unsigned>> possible_overlaps;
  {
    // Compute an AABB for each body
    std::vector<AABB> aabbs;
    aabbs.reserve( nbodies );
    for( unsigned bdy_idx = 0; bdy_idx < nbodies; ++bdy_idx )
    {
      Array2s min;
      Array2s max;
      m_state.bodyGeometry( bdy_idx )->computeCollisionAABB( q0.segment<2>( 3 * bdy_idx ), q0( 3 * bdy_idx + 2 ), q1.segment<2>( 3 * bdy_idx ), q1( 3 * bdy_idx + 2 ), min, max );
      aabbs.emplace_back( min, max );
    }
    assert( aabbs.size() == nbodies );

    // Determine which bodies possibly overlap
    SpatialGrid::getPotentialOverlaps( aabbs, possible_overlaps );
  }

  // Create constraints for bodies that actually overlap
  for( const auto& possible_overlap_pair : possible_overlaps )
  {
    assert( possible_overlap_pair.first < nbodies );
    assert( possible_overlap_pair.second < nbodies );

    // We can run standard narrow phase
    dispatchNarrowPhaseCollision( possible_overlap_pair.first, possible_overlap_pair.second, q0, q1, v, active_set );
  }
}
Exemplo n.º 3
0
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;
}
Exemplo n.º 4
0
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();
  }
}
Exemplo n.º 5
0
void IntegrationTools::exponentialEuler( const VectorXs& q0, const VectorXs& v0, const std::vector<bool>& fixed, const scalar& dt, VectorXs& q1 )
{
  assert( q0.size() == q1.size() );
  assert( q0.size() % 12 == 0 );
  assert( v0.size() * 2 == q0.size() );
  assert( 12 * fixed.size() == static_cast<unsigned long>(q0.size()) );

  const int nbodies{ static_cast<int>( fixed.size() ) };

  // Center of mass update
  for( int bdy_num = 0; bdy_num < nbodies; bdy_num++ )
  {
    if( !fixed[bdy_num] )
    {
      q1.segment<3>( 3 * bdy_num ) = q0.segment<3>( 3 * bdy_num ) + dt * v0.segment<3>( 3 * bdy_num );
    }
    else
    {
      q1.segment<3>( 3 * bdy_num ) = q0.segment<3>( 3 * bdy_num );
    }
  }

  // Orientation update
  for( int bdy_num = 0; bdy_num < nbodies; bdy_num++ )
  {
    if( !fixed[bdy_num] )
    {
      updateOrientation( nbodies, bdy_num, q0, v0, dt, q1 );
    }
    else
    {
      q1.segment<9>( 3 * nbodies + 9 * bdy_num ) = q0.segment<9>( 3 * nbodies + 9 * bdy_num );
    }
  }
}
Exemplo n.º 6
0
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();
  }
}
Exemplo n.º 7
0
void RigidBody2DSim::enforcePeriodicBoundaryConditions( VectorXs& q, VectorXs& v )
{
  assert( q.size() % 3 == 0 );
  assert( q.size() == v.size() );

  const unsigned nbodies{ static_cast<unsigned>( q.size() / 3 ) };

  // TODO: Probably faster to invert the loop here, only cache xin once per body
  // For each portal
  for( const PlanarPortal& planar_portal : m_state.planarPortals() )
  {
    // For each body
    for( unsigned bdy_idx = 0; bdy_idx < nbodies; ++bdy_idx )
    {
      const Vector2s xin{ q.segment<2>( 3 * bdy_idx ) };
      // TODO: Calling pointInsidePortal and teleportPointInsidePortal is a bit redundant, clean this up!
      // If the body is inside a portal
      if( planar_portal.pointInsidePortal( xin ) )
      {
        // Teleport to the other side of the portal
        Vector2s x_out;
        planar_portal.teleportPointInsidePortal( xin, x_out );
        q.segment<2>( 3 * bdy_idx ) = x_out;
        // TODO: This check probably isn't needed, additional_vel should be 0 for non-LE portals
        // Lees-Edwards Boundary conditions also update the velocity
        if( planar_portal.isLeesEdwards() )
        {
          const Vector2s additional_vel{ planar_portal.getKinematicVelocityOfPoint( xin ) };
          v.segment<2>( 3 * bdy_idx ) += additional_vel;
        }
      }
    }
  }
}
Exemplo n.º 8
0
void VortexForce::addHessXToTotal( const VectorXs& x, const VectorXs& v, const VectorXs& m, MatrixXs& hessE )
{
  assert( x.size() == v.size() );
  assert( x.size() == m.size() );
  assert( x.size() == hessE.rows() );
  assert( x.size() == hessE.cols() );
  assert( x.size()%2 == 0 );

//  scalar m1 = m(2*m_particles.second);
//  scalar m2 = m(2*m_particles.first);
//  
//  Vector2s nhat = x.segment<2>(2*m_particles.second)-x.segment<2>(2*m_particles.first); 
//  scalar r = nhat.norm(); 
//  assert( r != 0.0 ); 
//  nhat /= r;
//
//  Matrix2s entry = Matrix2s::Identity()-3.0*nhat*nhat.transpose();
//  entry *= m_G*m1*m2/r*r*r;
//  
//  hessE.block<2,2>(2*m_particles.first,2*m_particles.first)   += entry;
//  hessE.block<2,2>(2*m_particles.second,2*m_particles.second) += entry;
//  hessE.block<2,2>(2*m_particles.first,2*m_particles.second)  -= entry;
//  hessE.block<2,2>(2*m_particles.second,2*m_particles.first)  -= entry;

  std::cerr << outputmod::startred << "ERROR IN VORTEXFORCE: " << outputmod::endred << "No addHessXToTotal defined for VortexForce." << std::endl;
  exit(1);
}
Exemplo n.º 9
0
void RigidBody2DSim::linearInertialConfigurationUpdate( const VectorXs& q0, const VectorXs& v0, const scalar& dt, VectorXs& q1 ) const
{
  assert( q0.size() == v0.size() );
  assert( q0.size() == q1.size() );
  assert( dt > 0.0 );
  q1 = q0 + dt * v0;
}
// 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 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();
  }
}
Exemplo n.º 12
0
void DragDampingForce::addEnergyToTotal( const VectorXs& x, const VectorXs& v, const VectorXs& m, scalar& E )
{
  assert( x.size() == v.size() );
  assert( x.size() == m.size() );
  assert( x.size()%3 == 0 );

  std::cerr << outputmod::startred << "WARNING IN DRAGDAMPINGFORCE: " << outputmod::endred << "No energy defined for DragDampingForce." << std::endl;
}
Exemplo n.º 13
0
void SpringForce::addEnergyToTotal( const VectorXs& x, const VectorXs& v, const VectorXs& m, scalar& E )
{
  assert( x.size() == v.size() );
  assert( x.size()%2 == 0 );
  assert( m_endpoints.first >= 0 );  assert( m_endpoints.first < x.size()/2 );
  assert( m_endpoints.second >= 0 ); assert( m_endpoints.second < x.size()/2 );

  // Add milestone 2 code here.
}
Exemplo n.º 14
0
void FrictionOperatorUtilities::computeMDPLambda( const VectorXs& vrel, VectorXs& lambda )
{
  assert( vrel.size() % 2 == 0 );
  lambda.conservativeResize( vrel.size() / 2 );
  for( int con_num = 0; con_num < lambda.size(); ++con_num )
  {
    lambda( con_num ) = vrel.segment<2>( 2 * con_num ).norm();
  }
}
Exemplo n.º 15
0
void RigidBody2DSim::computeBodyPlaneActiveSetAllPairs( const VectorXs& q0, const VectorXs& q1, std::vector<std::unique_ptr<Constraint>>& active_set ) const
{
  assert( q0.size() % 3 == 0 ); assert( q0.size() == q1.size() );

  const unsigned nbodies{ static_cast<unsigned>( q0.size() / 3 ) };

  // Check all body-plane pairs
  for( unsigned plane_idx = 0; plane_idx < m_state.planes().size(); ++plane_idx )
  {
    const RigidBody2DStaticPlane& plane{ m_state.planes()[plane_idx] };
    for( unsigned bdy_idx = 0; bdy_idx < nbodies; ++bdy_idx )
    {
      // Skip kinematically scripted bodies
      if( isKinematicallyScripted( bdy_idx ) )
      {
        continue;
      }

      switch( m_state.geometry()[ m_state.geometryIndices()( bdy_idx ) ]->type() )
      {
        case RigidBody2DGeometryType::CIRCLE:
        {
          const CircleGeometry& circle_geo{ static_cast<CircleGeometry&>( *m_state.geometry()[ m_state.geometryIndices()( bdy_idx ) ] ) };
          if( StaticPlaneCircleConstraint::isActive( q1.segment<2>( 3 * bdy_idx ), circle_geo.r(), plane ) )
          {
            active_set.emplace_back( new StaticPlaneCircleConstraint{ bdy_idx, plane_idx, circle_geo.r(), plane } );
          }
          break;
        }
        case RigidBody2DGeometryType::BOX:
        {
          // TODO: Make this faster, if needed
          const BoxGeometry& box_geo{ static_cast<BoxGeometry&>( *m_state.geometry()[ m_state.geometryIndices()( bdy_idx ) ] ) };

          const Vector2s x{ q1.segment<2>( 3 * bdy_idx ) };
          const Eigen::Rotation2D<scalar> R{ q1( 3 * bdy_idx + 2 ) };
          const Array2s r{ box_geo.r() };

          // Check each vertex of the box
          for( int i = -1; i < 2; i += 2 )
          {
            for( int j = -1; j < 2; j += 2 )
            {
              const Vector2s body_space_arm{ ( Array2s{ i, j } * r ).matrix() };
              const Vector2s transformed_vertex{ x + R * body_space_arm };
              const scalar dist{ plane.n().dot( transformed_vertex - plane.x() ) };
              if( dist <= 0.0 )
              {
                active_set.emplace_back( new StaticPlaneBodyConstraint{ bdy_idx, body_space_arm, plane, plane_idx } );
              }
            }
          }
        }
      }
    }
  }
}
Exemplo n.º 16
0
void TwoDScene::accumulateddUdxdv( MatrixXs& A, const VectorXs& dx, const VectorXs& dv )
{
  assert( A.rows() == m_x.size() );
  assert( A.cols() == m_x.size() );
  assert( dx.size() == dv.size() );
  assert( dx.size() == 0 || dx.size() == A.rows() );

  if( dx.size() == 0 ) for( std::vector<Force*>::size_type i = 0; i < m_forces.size(); ++i ) m_forces[i]->addHessVToTotal( m_x, m_v, m_m, A );
  else                 for( std::vector<Force*>::size_type i = 0; i < m_forces.size(); ++i ) m_forces[i]->addHessVToTotal( m_x+dx, m_v+dv, m_m, A );
}
Exemplo n.º 17
0
void RigidBody2DSim::computeForce( const VectorXs& q, const VectorXs& v, const scalar& t, VectorXs& F )
{
  assert( q.size() % 3 == 0 ); assert( v.size() == q.size() ); assert( v.size() == F.size() );
  F.setZero();
  const std::vector<std::unique_ptr<RigidBody2DForce>>& forces{ m_state.forces() };
  for( const std::unique_ptr<RigidBody2DForce>& force : forces )
  {
    force->computeForce( q, v, m_state.M(), F );
  }
}
Exemplo n.º 18
0
void DragDampingForce::addHessXToTotal( const VectorXs& x, const VectorXs& v, const VectorXs& m, MatrixXs& hessE )
{
  assert( x.size() == v.size() );
  assert( x.size() == m.size() );
  assert( x.size() == hessE.rows() );
  assert( x.size() == hessE.cols() );
  assert( x.size()%3 == 0 );

  // Nothing to do.
}
Exemplo n.º 19
0
void VortexForce::addHessVToTotal( const VectorXs& x, const VectorXs& v, const VectorXs& m, MatrixXs& hessE )
{
  assert( x.size() == v.size() );
  assert( x.size() == m.size() );
  assert( x.size() == hessE.rows() );
  assert( x.size() == hessE.cols() );
  assert( x.size()%2 == 0 );

  std::cerr << outputmod::startred << "ERROR IN VORTEXFORCE: " << outputmod::endred << "No addHessXToTotal defined for VortexForce." << std::endl;
  exit(1);
}
Exemplo n.º 20
0
VectorXs BodyBodyConstraint::computeRelativeVelocity( const VectorXs& q, const VectorXs& v ) const
{
  assert( v.size() % 6 == 0 );
  assert( v.size() / 2 + 3 * m_idx0 + 2 < v.size() );
  assert( v.size() / 2 + 3 * m_idx1 + 2 < v.size() );

  const unsigned nbodies{ static_cast<unsigned>( v.size() / 6 ) };

  // v_j + omega_j x r_j - ( v_i + omega_i x r_i )
  return v.segment<3>( 3 * m_idx0 ) + v.segment<3>( 3 * ( nbodies + m_idx0 ) ).cross( m_r0 ) - v.segment<3>( 3 * m_idx1 ) - v.segment<3>( 3 * ( nbodies + m_idx1 ) ).cross( m_r1 );
}
Exemplo n.º 21
0
scalar RigidBody::computeMomentOfInertia( const VectorXs& vertices, const VectorXs& masses ) const
{
  assert( vertices.size()%2 == 0 );
  assert( 2*masses.size() == vertices.size() );
  
  // COMPLETE THIS CODE
	scalar I = 0;
	for(int i=0;i<masses.size();i++) {
		I += masses(i)*(m_X-vertices.segment<2>(2*i)).squaredNorm();
	}
  return I;
}
Exemplo n.º 22
0
scalar RigidBody::computeMomentOfInertia( const VectorXs& vertices, const VectorXs& masses ) const
{
  assert( vertices.size()%2 == 0 );
  assert( 2*masses.size() == vertices.size() );

  // TODO: By using a partial reduction from Eigen, we could make this a single line of vectorized code :). 
  
  Vector2s cm = computeCenterOfMass(vertices,masses);
  scalar I = 0.0;
  for( int i = 0; i < masses.size(); ++i ) I += masses(i)*(vertices.segment<2>(2*i)-cm).squaredNorm();
  return I;
}
VectorXs StaticPlaneSphereConstraint::computeRelativeVelocity( const VectorXs& q, const VectorXs& v ) const
{
  assert( v.size() % 6 == 0 );
  assert( v.size() / 2 + 3 * m_sphere_idx + 2 < v.size() );

  const unsigned nbodies{ static_cast<unsigned>( v.size() / 6 ) };

  const Vector3s r_sphere{ - m_r * m_plane.n() };

  // v_point + omega_point x r_point - v_plane_collision_point
  return v.segment<3>( 3 * m_sphere_idx ) + v.segment<3>( 3 * ( nbodies + m_sphere_idx ) ).cross( r_sphere ) - computePlaneCollisionPointVelocity( q );
}
Exemplo n.º 24
0
static SparseMatrixsc createMinv( const VectorXs& m )
{
  SparseMatrixsc Minv{ static_cast<SparseMatrixsc::Index>( m.size() ), static_cast<SparseMatrixsc::Index>( m.size() ) };
  Minv.reserve( SparseMatrixsc::Index( m.size() ) );
  for( int col = 0; col < m.size(); ++col )
  {
    Minv.startVec( col );
    const int row = col;
    Minv.insertBack( row, col ) = 1.0 / m( col );
  }
  Minv.finalize();
  return Minv;
}
Exemplo n.º 25
0
Vector2s RigidBody::computeCenterOfMass( const VectorXs& vertices, const VectorXs& masses ) const
{
  // COMPLETE THIS CODE
	assert( vertices.size()%2 == 0 );
	assert( 2*masses.size() == vertices.size() );
	Vector2s com;
	com.setZero();
	for(int i=0;i<masses.size();i++) {
		com+=masses(i)*vertices.segment<2>(2*i);
	}
	com /= m_M;
  return com;
}
Exemplo n.º 26
0
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;
}
Exemplo n.º 27
0
void BodyBodyConstraint::computeFrictionMask( const int nbodies, VectorXs& friction_mask ) const
{
  assert( 3 * m_idx0 + 2 < friction_mask.size() );
  friction_mask.segment<3>( 3 * m_idx0 ).setConstant( 1.0 );

  assert( 3 * ( m_idx0 + nbodies ) + 2 < friction_mask.size() );
  friction_mask.segment<3>( 3 * ( m_idx0 + nbodies ) ).setConstant( 1.0 );

  assert( 3 * m_idx1 + 2 < friction_mask.size() );
  friction_mask.segment<3>( 3 * m_idx1 ).setConstant( 1.0 );

  assert( 3 * ( m_idx1 + nbodies ) + 2 < friction_mask.size() );
  friction_mask.segment<3>( 3 * ( m_idx1 + nbodies ) ).setConstant( 1.0 );
}
Exemplo n.º 28
0
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 );
    }
  }
}
Exemplo n.º 29
0
scalar RigidBody2DSim::computeTotalAngularMomentum() const
{
  VectorXs L;
  computeAngularMomentum( m_state.v(), L );
  assert( L.size() == 1 );
  return L( 0 );
}
Exemplo n.º 30
0
Vector2s RigidBody::computeCenterOfMass( const VectorXs& vertices, const VectorXs& masses ) const
{
  assert( vertices.size()%2 == 0 );
  assert( 2*masses.size() == vertices.size() );

  // TODO: By using a partial reduction from Eigen, we could make this a single line of vectorized code :). 
  
  Vector2s cm(0.0,0.0);
  for( int i = 0; i < vertices.size()/2; ++i ) cm += masses(i)*vertices.segment<2>(2*i);
  
  scalar M = masses.sum();  
  assert( M > 0.0 );
  cm /= M;

  return cm;
}