Esempio n. 1
0
// Adds the gradient of the penalty potential (-1 * force) for a particle-edge
// pair to the total.
// Read the positions of the particle and edge endpoints from the input
// variable x.
// Inputs:
//   x:    The positions of the particles in the scene.
//   vidx: The index of the particle.
//   eidx: The index of the edge, i.e. the indices of the particle making up the
//         endpoints of the edge are given by m_scene.getEdge(eidx).first and 
//         m_scene.getEdges(eidx).second.
// Outputs:
//   gradE: The total gradient of penalty force. *ADD* the particle-edge
//          gradient to this total gradient.
void PenaltyForce::addParticleEdgeGradEToTotal(const VectorXs &x, int vidx, int eidx, VectorXs &gradE)
{
    VectorXs x1 = x.segment<2>(2*vidx);
    VectorXs x2 = x.segment<2>(2*m_scene.getEdge(eidx).first);
    VectorXs x3 = x.segment<2>(2*m_scene.getEdge(eidx).second);
    
    double r1 = m_scene.getRadius(vidx);
    double r2 = m_scene.getEdgeRadii()[eidx];
    
    // your implementation here
	int eidx1 = m_scene.getEdge(eidx).first;
	int eidx2 = m_scene.getEdge(eidx).second;
	scalar alpha = (x1-x2).dot(x3-x2)/(x3-x2).squaredNorm();
	if(alpha<0) alpha = 0;
	if(alpha>1) alpha = 1;
	Vector2s n = alpha*x3+(1-alpha)*x2-x1;
	if(n.norm()>r1+r2+m_thickness) return;
	Vector2s nhat = n/n.norm();
	MatrixXs gradn(2, 6);
	Matrix2s I = I.Identity();
	gradn<<-I, (1-alpha)*I, alpha*I;
	VectorXs gradV = m_k*(n.norm()-r1-r2-m_thickness)*gradn.transpose()*nhat;
	gradE(2*vidx) += gradV(0);
	gradE(2*vidx+1) += gradV(1);
	gradE(2*eidx1) += gradV(2);
	gradE(2*eidx1+1) += gradV(3);
	gradE(2*eidx2) += gradV(4);
	gradE(2*eidx2+1) += gradV(5);
    
}
Esempio n. 2
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;
}
Esempio n. 3
0
static PyObject* insertBall( PyObject* self, PyObject* args )
{
  Vector2s q;
  Vector2s v;
  scalar r;
  scalar m;
  int fixed;
  assert( args != nullptr );
  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." );
  if( !PyArg_ParseTuple( args, is_same<scalar,double>::value ? "ddddddi" : "ffffffi", &q.x(), &q.y(), &v.x(), &v.y(), &r, &m, &fixed ) )
  {
    PyErr_Print();
    std::cerr << "Failed to read parameters for insertBall, parameters are: double x, double y, double vx, double vy, double radius, double mass, bool fixed. Exiting." << std::endl;
    std::exit( EXIT_FAILURE );
  }
  if( r <= 0.0 )
  {
    std::cerr << "Error in insert ball, radius must be positive. Exiting." << std::endl;
    std::exit( EXIT_FAILURE );
  }
  if( m <= 0.0 )
  {
    std::cerr << "Error in insert ball, mass must be positive. Exiting." << std::endl;
    std::exit( EXIT_FAILURE );
  }
  if( fixed != 0 && fixed != 1 )
  {
    std::cerr << "Error in insert ball, fixed value must be 0 or 1. Exiting." << std::endl;
    std::exit( EXIT_FAILURE );
  }
  assert( s_ball_state != nullptr );
  s_ball_state->pushBallBack( q, v, r, m, fixed );
  return Py_BuildValue( "" );
}
void TeleportedCircleCircleConstraint::computeContactBasis( const VectorXs& q, const VectorXs& v, MatrixXXsc& basis ) const
{
  assert( fabs( m_n.norm() - 1.0 ) <= 1.0e-6 );
  const Vector2s t{ -m_n.y(), m_n.x() };
  assert( fabs( t.norm() - 1.0 ) <= 1.0e-6 ); assert( fabs( m_n.dot( t ) ) <= 1.0e-6 );
  basis.resize( 2, 2 );
  basis.col( 0 ) = m_n;
  basis.col( 1 ) = t;
}
Esempio n. 5
0
bool MathUtilities::isRightHandedOrthoNormal( const Vector2s& a, const Vector2s& b, 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; }
  // All basis vectors should be mutually orthogonal
  if( fabs( a.dot( b ) ) > tol ) { return false; }
  // Coordinate system should be right handed
  assert( fabs( cross( a, b ) - 1.0 ) <= 1.0e-6 || fabs( cross( a, b ) + 1.0 ) <= 1.0e-6 );
  if( cross( a, b ) <= 0.0 ) { return false; }
  return true;
}
Esempio n. 6
0
void TwoDSceneSVGRenderer::renderSolidCircle( std::fstream& file, const Vector2s& center, const scalar& r, const renderingutils::Color& color ) const
{
    file << "<circle fill=\"#";
    file << intToHexString(floor(255.0*color.r+0.5)) << intToHexString(floor(255.0*color.g+0.5)) << intToHexString(floor(255.0*color.b+0.5)) << "\"" << " stroke=\"#";
    file << intToHexString(floor(255.0*color.r+0.5)) << intToHexString(floor(255.0*color.g+0.5)) << intToHexString(floor(255.0*color.b+0.5)) << "\"" << " stroke-width=\"0\" cx=\"";
    file << center.x();
    file << "\" cy=\"";
    file << center.y();
    file << "\" r=\"";
    file << r;
    file << "\"/>" << std::endl;
}
Esempio n. 7
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;
}
Esempio n. 8
0
// Adds the gradient of the penalty potential (-1 * force) for a particle-
// half-plane pair to the total.
// Read the positions of the particle from the input variable x.
// Inputs:
//   x:    The positions of the particles in the scene.
//   vidx: The index of the particle.
//   pidx: The index of the half-plane, i.e. the position and normal vectors
//         for the half-plane can be retrieved by calling
//         m_scene.getHalfplane(pidx).
// Outputs:
//   gradE: The total gradient of the penalty force. *ADD* the particle-
//          half-plane gradient to this total gradient.
void PenaltyForce::addParticleHalfplaneGradEToTotal(const VectorXs &x, int vidx, int pidx, VectorXs &gradE)
{
    VectorXs x1 = x.segment<2>(2*vidx);
    VectorXs nh = m_scene.getHalfplane(pidx).second;
	VectorXs px = m_scene.getHalfplane(pidx).first;
    
    // your implementation here
	Vector2s n = (px-x1).dot(nh)/nh.squaredNorm()*nh;
	Vector2s nhat = n/n.norm();
	scalar r1 = m_scene.getRadius(vidx);
	if(n.norm()>r1+m_thickness) return;
	Matrix2s gradn = -nh*nh.transpose()/nh.squaredNorm();
	Vector2s gradV = m_k*(n.norm()-r1-m_thickness)*gradn.transpose()*nhat;
	gradE(2*vidx) += gradV(0);
	gradE(2*vidx+1) += gradV(1);
}
Esempio n. 9
0
Vector2s vavImage::GetWhitePoints()
{
	Vector2s out;
	for (int j = 0; j < m_Image.cols; ++j)
	{
		for (int i = 0; i < m_Image.rows; ++i)
		{
			cv::Vec3b intensity = m_Image.at<cv::Vec3b>(i, j);
			if (intensity[0] != 0 || intensity[1] != 0 || intensity[2] != 0)
			{
				out.push_back(Vector2(i, j));
				//std::cout << i << " " << j << std::endl;
			}
		}
	}
	return out;
}
Esempio n. 10
0
// TODO: Don't do the if here, have children do what they need to do
scalar Constraint::computeLambda( const VectorXs& q, const VectorXs& v ) const
{
  MatrixXXsc basis;
  computeBasis( q, v, basis );
  assert( basis.rows() == basis.cols() );
  assert( basis.cols() == 2 || basis.cols() == 3 );
  const VectorXs rel_vel = computeRelativeVelocity( q, v );
  assert( rel_vel.size() == basis.rows() );
  if( basis.cols() == 3 )
  {
    const Vector2s tangent_vel( rel_vel.dot( basis.col( 1 ) ), rel_vel.dot( basis.col( 2 ) ) );
    return tangent_vel.norm();
  }
  else if( basis.cols() == 2 )
  {
    return fabs( rel_vel.dot( basis.col( 1 ) ) );
  }
  std::cerr << "Unhandled case in Constraint::computeLambda" << std::endl;
  std::exit( EXIT_FAILURE );
}
Esempio n. 11
0
// Adds the gradient of the penalty potential (-1 * force) for a pair of 
// particles to the total.
// Read the positions of the particles from the input variable x. Radii can
// be obtained from the member variable m_scene, the penalty force stiffness 
// from member variable m_k, and penalty force thickness from member variable
// m_thickness.
// Inputs:
//   x:    The positions of the particles in the scene. 
//   idx1: The index of the first particle, i.e. the position of this particle
//         is ( x[2*idx1], x[2*idx1+1] ).
//   idx2: The index of the second particle.
// Outputs:
//   gradE: The total gradient of penalty force. *ADD* the particle-particle
//          gradient to this total gradient.
void PenaltyForce::addParticleParticleGradEToTotal(const VectorXs &x, int idx1, int idx2, VectorXs &gradE)
{
    VectorXs x1 = x.segment<2>(2*idx1);
    VectorXs x2 = x.segment<2>(2*idx2);
    
    double r1 = m_scene.getRadius(idx1);
    double r2 = m_scene.getRadius(idx2);
    
    // your implementation here
	Vector2s n = x2-x1;
	if(n.norm()>r1+r2+m_thickness) return;
	Vector2s nhat = n/n.norm();
	MatrixXs gradn(2, 4);
	Matrix2s I = I.Identity();
	gradn<<-I, I;
	VectorXs gradV = m_k*(n.norm()-r1-r2-m_thickness)*gradn.transpose()*nhat;
	gradE(2*idx1) += gradV(0);
	gradE(2*idx1+1) += gradV(1);
	gradE(2*idx2) += gradV(2);
	gradE(2*idx2+1) += gradV(3);
}
Esempio n. 12
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()%2 == 0 );
  assert( m_particles.first >= 0 );  assert( m_particles.first < x.size()/2 );
  assert( m_particles.second >= 0 ); assert( m_particles.second < x.size()/2 );

  Vector2s rhat = x.segment<2>(2*m_particles.second)-x.segment<2>(2*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<2>(2*m_particles.second)-v.segment<2>(2*m_particles.first);
  
  rhat /= r*r;
  rhat *= m_kvc;

  gradE.segment<2>(2*m_particles.first)  -= -rhat;
  gradE.segment<2>(2*m_particles.second) += -rhat;
}
Esempio n. 13
0
static VectorXs parallelTransport2D( const VectorXs& n0, const VectorXs& n1, const VectorXs& t0 )
{
  assert( fabs( n0.norm() - 1.0 ) <= 1.0e-6 ); assert( fabs( n1.norm() - 1.0 ) <= 1.0e-6 );

  // x is cos of angle, y is sin of angle
  const Vector2s r{ n0.dot( n1 ), MathUtilities::cross( n0, n1 ) };
  assert( fabs( r.norm() - 1.0 ) <= 1.0e-6 );

  // Rotate t0
  VectorXs t1{ 2 };
  t1( 0 ) = r.x() * t0.x() - r.y() * t0.y();
  t1( 1 ) = r.y() * t0.x() + r.x() * t0.y();
  assert( fabs( t0.norm() - t1.norm() ) <= 1.0e-6 );
  // TODO: Assert n0,t0 angle is same as n1,t1 angle

  return t1;
}
Esempio n. 14
0
void TwoDSceneSVGRenderer::renderRigidBodyScene( const std::string& name, const RigidBodyScene& rbscene ) const
{
//  const VectorXs& x = m_scene.getX();
//  assert( x.size()%2 == 0 );
//  assert( 2*m_scene.getNumParticles() == x.size() );
//  int numparticles = x.size()/2;
//  const std::vector<scalar>& radii = m_scene.getRadii();
//  assert( numparticles == (int) radii.size() );

    std::fstream file(name.c_str(), std::fstream::out);
    if(!file)
    {
        std::cerr << "Failure writing SVG file!" << std::endl;
        exit(1);
    }

    scalar scale, xmin, ymin, xshift, yshift;
    computeSimToImageMap( scale, xmin, ymin, xshift, yshift );

    file << "<?xml version=\"1.0\" encoding=\"utf-8\"?> <!-- Generator: Adobe Illustrator 13.0.0, SVG Export Plug-In . SVG Version: 6.00 Build 14948)  --> <svg version=\"1.2\" baseProfile=\"tiny\" id=\"Layer_1\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\" x=\"0px\" y=\"0px\" width=\"";
    file << m_w;
    file << "px\" height=\"";
    file << m_h;
    file << "px\" viewBox=\"0 0 ";
    file << m_w << " " << m_h;
    file << "\" xml:space=\"preserve\">" << std::endl;

    // Simulate a background color by rendering a large colored quad
    file << "<polygon points=\"" << 0 << "," << 0 << " " << m_w << "," << 0 << " " << m_w << "," << m_h << " " << 0 << "," << m_h;
    file << "\" style=\"fill:#" << intToHexString(floor(255.0*m_bgcolor.r+0.5)) << intToHexString(floor(255.0*m_bgcolor.g+0.5)) << intToHexString(floor(255.0*m_bgcolor.b+0.5));
    file << "; stroke:#000000;stroke-width:0\"/>" << std::endl;

    const std::vector<RigidBody> rbs = rbscene.getRigidBodies();
    assert( m_particle_colors.size() == rbs.size() );

    // For each rigid body
    for( std::vector<RigidBody>::size_type i = 0; i < rbs.size(); ++i )
    {
        scalar r = rbs[i].getRadius();
        renderingutils::Color rbcolor = m_particle_colors[i];
        // Render each edge of each rigid body
        for( int j = 0; j < rbs[i].getNumVertices(); ++j )
        {
            const Vector2s& xi = rbs[i].getWorldSpaceVertex(j);
            const Vector2s& xj = rbs[i].getWorldSpaceVertex((j+1)%rbs[i].getNumVertices());

            Vector2s p0;
            p0 << scale*(xi.x()-xmin) + xshift, ((scalar)m_h) - scale*(xi.y()-ymin) - yshift;

            Vector2s p1;
            p1 << scale*(xj.x()-xmin) + xshift, ((scalar)m_h) - scale*(xj.y()-ymin) - yshift;

            renderSweptEdge( file, p0, p1, scale*r, rbcolor );
        }
    }

    // Render each spring force
    for( std::vector<RigidBody>::size_type i = 0; i < m_svg_renderers.size(); ++i )
    {
        renderingutils::Color clr = m_svg_renderers[i]->getColor();

        Vector2s xi = m_svg_renderers[i]->getVertexOne(rbs);
        Vector2s xj = m_svg_renderers[i]->getVertexTwo(rbs);

        Vector2s p0;
        p0 << scale*(xi.x()-xmin) + xshift, ((scalar)m_h) - scale*(xi.y()-ymin) - yshift;

        Vector2s p1;
        p1 << scale*(xj.x()-xmin) + xshift, ((scalar)m_h) - scale*(xj.y()-ymin) - yshift;

        renderSweptEdge( file, p0, p1, scale*0.03, clr );
    }


    file << "</svg>" << std::endl;

    file.close();
}
Esempio n. 15
0
void TwoDSceneSVGRenderer::renderShared( std::fstream& file, const VectorXs& x, const std::vector<std::pair<int,int> >& edges, const std::vector<scalar>& radii, const std::vector<scalar>& edgeradii, const scalar& scale, const scalar& xmin, const scalar& ymin, const scalar& xshift, const scalar& yshift  ) const
{
    int numparticles = x.size()/2;

    for( std::vector<renderingutils::ParticlePath>::size_type i = 0; i < m_particle_paths.size(); ++i )
    {
        const std::list<Vector2s>& ppath = m_particle_paths[i].getPath();
        const renderingutils::Color& pathcolor = m_particle_paths[i].getColor();

        file << "<polyline points=\"";

        for( std::list<Vector2s>::const_iterator itr = ppath.begin(); itr != ppath.end(); ++itr )
        {
            Vector2s point;
            point << scale*(itr->x()-xmin) + xshift, ((scalar)m_h) - scale*(itr->y()-ymin) - yshift;
            file << point.x() << "," << point.y() << " ";
        }

        file << "\" style=\"fill:none;stroke:#";
        file << intToHexString(floor(255.0*pathcolor.r+0.5)) << intToHexString(floor(255.0*pathcolor.g+0.5)) << intToHexString(floor(255.0*pathcolor.b+0.5));
        file << ";stroke-width:1\"/>" << std::endl;
    }

    // Render edges
    assert( edgeradii.size() == edges.size() );
    for( std::vector<std::pair<int,int> >::size_type i = 0; i < edges.size(); ++i )
    {
        assert( edges[i].first >= 0 );
        assert( edges[i].first < m_scene.getNumParticles() );
        assert( edges[i].second >= 0 );
        assert( edges[i].second < m_scene.getNumParticles() );

        int i0 = edges[i].first;
        int i1 = edges[i].second;

        Vector2s p0;
        p0 << scale*(x(2*i0)-xmin) + xshift, ((scalar)m_h) - scale*(x(2*i0+1)-ymin) - yshift;

        Vector2s p1;
        p1 << scale*(x(2*i1)-xmin) + xshift, ((scalar)m_h) - scale*(x(2*i1+1)-ymin) - yshift;

        renderSweptEdge( file, p0, p1, scale*edgeradii[i], m_edge_colors[i] );
    }

    // Render halfplanes
    for(int i=0; i < m_scene.getNumHalfplanes(); i++)
    {
        Vector2s p0;
        p0 << scale * (m_scene.getHalfplane(i).first[0]-xmin) + xshift, ((scalar)m_h) - scale*(m_scene.getHalfplane(i).first[1]-ymin) - yshift;
        Vector2s n0;
        n0 << m_scene.getHalfplane(i).second[0], -m_scene.getHalfplane(i).second[1];
        n0.normalize();
        renderHalfplane(file, p0, n0, m_halfplane_colors[i]);
    }

    // Render particles
    for( int i = 0; i < numparticles; ++i )
    {
        Vector2s center;
        center << scale*(x(2*i)-xmin) + xshift, ((scalar)m_h) - scale*(x(2*i+1)-ymin) - yshift;
        renderSolidCircle( file, center, scale*radii[i], m_particle_colors[i] );
    }
}
Esempio n. 16
0
void TwoDSceneSVGRenderer::renderSweptEdge( std::fstream& file, const Vector2s& x0, const Vector2s& x1, const scalar& r, const renderingutils::Color& color ) const
{
    scalar theta = atan2(x1.y()-x0.y(),x1.x()-x0.x());

    scalar rx = -r*sin(theta);
    scalar ry =  r*cos(theta);

    scalar p0x = x0.x() + rx;
    scalar p0y = x0.y() + ry;
    scalar p1x = x0.x() - rx;
    scalar p1y = x0.y() - ry;
    scalar p2x = x1.x() - rx;
    scalar p2y = x1.y() - ry;
    scalar p3x = x1.x() + rx;
    scalar p3y = x1.y() + ry;

    file << "<polygon points=\"" << p0x << "," << p0y << " " << p1x << "," << p1y << " " << p2x << "," << p2y << " " << p3x << "," << p3y;
    file << "\" style=\"fill:#" << intToHexString(floor(255.0*color.r+0.5)) << intToHexString(floor(255.0*color.g+0.5)) << intToHexString(floor(255.0*color.b+0.5));
    file << "; stroke:#000000;stroke-width:0\"/>" << std::endl;

    renderSolidCircle( file, x0, r, color );
    renderSolidCircle( file, x1, r, color );
}