C++ TwoDScene::getV方法代码示例

void TwoDSceneGrader::addToAccumulatedResidual( const TwoDScene& oracle_scene, const TwoDScene& testing_scene )
{
  assert(   oracle_scene.getNumParticles() == testing_scene.getNumParticles() );
  assert( 2*oracle_scene.getNumParticles() == oracle_scene.getX().size() );
  assert( 2*oracle_scene.getNumParticles() == testing_scene.getX().size() );
  assert( 2*oracle_scene.getNumParticles() == oracle_scene.getV().size() );
  assert( 2*oracle_scene.getNumParticles() == testing_scene.getV().size() );
  
  const VectorXs& oracle_x = oracle_scene.getX();
  const VectorXs& testing_x = testing_scene.getX();
  
  const VectorXs& oracle_v = oracle_scene.getV();
  const VectorXs& testing_v = testing_scene.getV();
  
  for( int i = 0; i < oracle_scene.getNumParticles(); ++i )
  {
    scalar x_resid = (oracle_x.segment<2>(2*i)-testing_x.segment<2>(2*i)).norm();
    assert( x_resid >= 0.0 );
    scalar v_resid = (oracle_v.segment<2>(2*i)-testing_v.segment<2>(2*i)).norm();
    assert( v_resid >= 0.0 );
    m_accumulated_position_residual += x_resid;
    m_accumulated_velocity_residual += v_resid;
    if( x_resid > m_max_position_residual ) m_max_position_residual = x_resid;
    if( v_resid > m_max_velocity_residual ) m_max_velocity_residual = v_resid;
  }
}

// Responds to a collision detected between a particle and a half-plane by 
// applying an impulse to the velocity of the particle.
// Inputs:
//   scene: The scene data structure.
//   vidx:  The index of the particle.
//   pidx:  The index of the half-plane.
//   n:     The shortest vector between the particle and the half-plane.
// Outputs:
//   None.
void SimpleCollisionHandler::respondParticleHalfplane(TwoDScene &scene, int vidx, int pidx, const VectorXs &n)
{
  VectorXs nhat = n;
  nhat.normalize();
  double cfactor = (1.0+getCOR())/2.0;
  scene.getV().segment<2>(2*vidx) -= 2*cfactor*scene.getV().segment<2>(2*vidx).dot(nhat)*nhat;
}

// Detects whether two particles are overlapping (including the radii of each)
// and approaching.
// If the two particles overlap and are approaching, returns true and sets 
// the vector n to be the vector between the first and second particle.
// Inputs:
//   scene: The scene data structure. The positions and radii of the particles
//          can be obtained from here.
//   idx1:  The index of the first particle. (Ie, the degrees of freedom
//          corresponding to this particle are entries 2*idx1 and 2*idx1+1 in
//          scene.getX().
//   idx2:  The index of the second particle.
// Outputs:
//   n: The vector between the two particles.
//   Returns true if the two particles overlap and are approaching.
bool SimpleCollisionHandler::detectParticleParticle(TwoDScene &scene, int idx1, int idx2, VectorXs &n)
{
  VectorXs x1 = scene.getX().segment<2>(2*idx1);
  VectorXs x2 = scene.getX().segment<2>(2*idx2);
  n = x2-x1;
  if(n.norm() < scene.getRadius(idx1) + scene.getRadius(idx2))
    {
      double relvel = (scene.getV().segment<2>(2*idx1)-scene.getV().segment<2>(2*idx2)).dot(n);
      if(relvel > 0)
	return true;
    }
  return false;
}

// Responds to a collision detected between a particle and an edge by applying
// an impulse to the velocities of each one.
// Inputs:
//   scene: The scene data structure.
//   vidx:  The index of the particle.
//   eidx:  The index of the edge.
//   n:     The shortest vector between the particle and the edge.
// Outputs:
//   None.
void SimpleCollisionHandler::respondParticleEdge(TwoDScene &scene, int vidx, int eidx, const VectorXs &n)
{
  const VectorXs &M = scene.getM();

  int eidx1 = scene.getEdges()[eidx].first;
  int eidx2 = scene.getEdges()[eidx].second;

  VectorXs x1 = scene.getX().segment<2>(2*vidx);
  VectorXs x2 = scene.getX().segment<2>(2*eidx1);
  VectorXs x3 = scene.getX().segment<2>(2*eidx2);
  
  VectorXs v1 = scene.getV().segment<2>(2*vidx);
  VectorXs v2 = scene.getV().segment<2>(2*eidx1);
  VectorXs v3 = scene.getV().segment<2>(2*eidx2);

  VectorXs nhat = n;
  nhat.normalize();

  double alpha = (x1-x2).dot(x3-x2)/(x3-x2).dot(x3-x2);
  alpha = std::min(1.0, std::max(0.0, alpha) );
  VectorXs vedge = v2 + alpha*(v3-v2);
  double cfactor = (1.0 + getCOR())/2.0;

  double m1 = scene.isFixed(vidx) ? std::numeric_limits<double>::infinity() : M[2*vidx];
  double m2 = scene.isFixed(eidx1) ? std::numeric_limits<double>::infinity() : M[2*eidx1];
  double m3 = scene.isFixed(eidx2) ? std::numeric_limits<double>::infinity() : M[2*eidx2];

  double numerator = 2*cfactor*(vedge-v1).dot(nhat);
  double denom1 = 1.0 + (1-alpha)*(1-alpha)*m1/m2 + alpha*alpha*m1/m3;
  double denom2 = m2/m1 + (1-alpha)*(1-alpha) + alpha*alpha*m2/m3;
  double denom3 = m3/m1 + (1-alpha)*(1-alpha)*m3/m2 + alpha*alpha;

  if(!scene.isFixed(vidx))
    scene.getV().segment<2>(2*vidx) += numerator/denom1 * nhat;
  if(!scene.isFixed(eidx1))
    scene.getV().segment<2>(2*eidx1) -= (1.0-alpha)*numerator/denom2 * nhat;
  if(!scene.isFixed(eidx2))
    scene.getV().segment<2>(2*eidx2) -= alpha * numerator/denom3 * nhat;
}

// Detects whether a particle and a half-plane are overlapping (including the 
// radius of the particle) and are approaching.
// If the two objects overlap and are approaching, returns true and sets the 
// vector n to be the shortest vector between the particle and the half-plane.
// Inputs:
//   scene: The scene data structure.
//   vidx:  The index of the particle.
//   pidx:  The index of the halfplane. The vectors (px, py) and (nx, ny) can
//          be retrieved by calling scene.getHalfplane(pidx).
// Outputs:
//   n: The shortest vector between the particle and the half-plane.
//   Returns true if the two objects overlap and are approaching.
bool SimpleCollisionHandler::detectParticleHalfplane(TwoDScene &scene, int vidx, int pidx, VectorXs &n)
{
  VectorXs x1 = scene.getX().segment<2>(2*vidx);
  VectorXs px = scene.getHalfplane(pidx).first;
  VectorXs pn = scene.getHalfplane(pidx).second;
  pn.normalize();
  n = (px-x1).dot(pn)*pn;
  if(n.norm() < scene.getRadius(vidx))
    {
      double relvel = scene.getV().segment<2>(2*vidx).dot(n);
      if(relvel > 0)
	return true;
    }
  return false;
}

// Detects whether a particle and an edge are overlapping (including the radii 
// of both) and are approaching.
// If the two objects overlap and are approaching, returns true and sets the 
// vector n to be the shortest vector between the particle and the edge.
// Inputs:
//   scene: The scene data structure.
//   vidx:  The index of the particle.
//   eidx:  The index of the edge. (Ie, the indices of particle with index e are
//          scene.getEdges()[e].first and scene.getEdges()[e].second.)
// Outputs:
//   n: The shortest vector between the particle and the edge.
//   Returns true if the two objects overlap and are approaching.
bool SimpleCollisionHandler::detectParticleEdge(TwoDScene &scene, int vidx, int eidx, VectorXs &n)
{
  VectorXs x1 = scene.getX().segment<2>(2*vidx);
  VectorXs x2 = scene.getX().segment<2>(2*scene.getEdges()[eidx].first);
  VectorXs x3 = scene.getX().segment<2>(2*scene.getEdges()[eidx].second);
  double alpha = (x1-x2).dot(x3-x2)/(x3-x2).dot(x3-x2);
  alpha = std::min(1.0, std::max(0.0, alpha));

  VectorXs closest = x2 + alpha*(x3-x2);
  n = closest-x1;

  if(n.norm() < scene.getRadius(vidx)+scene.getEdgeRadii()[eidx]) 
    {
      VectorXs v1 = scene.getV().segment<2>(2*vidx);
      VectorXs v2 = scene.getV().segment<2>(2*scene.getEdges()[eidx].first);
      VectorXs v3 = scene.getV().segment<2>(2*scene.getEdges()[eidx].second);
      double relvel = (v1 - v2 - alpha*(v3-v2)).dot(n);
      if(relvel > 0)
	{
	  return true;
	}
    }
  return false;
}

bool LinearizedImplicitEuler::stepScene( TwoDScene& scene, scalar dt )
{
  VectorXs& x = scene.getX();
  VectorXs& v = scene.getV();
  const VectorXs& m = scene.getM();
  assert(x.size() == v.size());
  assert(x.size() == m.size());

  // Implement implicit euler here!
	VectorXs F(x.size());
	F.setZero();
	scene.accumulateGradU(F, dt*v, VectorXs(x.size()).setZero());
	// Force is negative the energy gradient
	F *= -1.0;

	MatrixXs M(x.size(), x.size());
	M.setZero();
	for (int i=0;i<x.size();i+=2) {
		M(i, i) = m[i];
		M(i+1, i+1) = m[i+1];
	}
	MatrixXs MatQ(x.size(), x.size());	// dF/dq
	MatQ.setZero();
	scene.accumulateddUdxdx(MatQ, dt*v, VectorXs(x.size()).setZero());
	MatrixXs MatV(x.size(), x.size());	// dF/dv
	MatV.setZero();
	scene.accumulateddUdxdv(MatV, dt*v, VectorXs(x.size()).setZero());
	MatrixXs A = M-(dt*dt*MatQ+dt*MatV);
	// Zero the force for fixed DoFs
	for( int i = 0; i < scene.getNumParticles(); ++i ) if( scene.isFixed(i) ) F.segment<2>(2*i).setZero();
	for( int i = 0; i < scene.getNumParticles(); ++i ) if( scene.isFixed(i) ) {
		A.row(2*i).setZero();
		A.row(2*i+1).setZero();
		A.col(2*i).setZero();
		A.col(2*i+1).setZero();
		A(2*i, 2*i) = 1;
		A(2*i+1, 2*i+1) = 1;
	}
	VectorXs dv = A.fullPivLu().solve(dt*F);
	v = v+dv;
	x = x+v*dt;
  
  return true;
}

// Responds to a collision detected between two particles by applying an impulse
// to the velocities of each one.
// You can get the COR of the simulation by calling getCOR().
// Inputs:
//   scene: The scene data structure.
//   idx1:  The index of the first particle.
//   idx2:  The index of the second particle.
//   n:     The vector between the first and second particle.
// Outputs:
//   None.
void SimpleCollisionHandler::respondParticleParticle(TwoDScene &scene, int idx1, int idx2, const VectorXs &n)
{
  const VectorXs &M = scene.getM();
  VectorXs &v = scene.getV();

  VectorXs nhat = n;
  nhat.normalize();

  double cfactor = (1.0 + getCOR())/2.0;
  double m1 = scene.isFixed(idx1) ? std::numeric_limits<double>::infinity() : M[2*idx1];
  double m2 = scene.isFixed(idx2) ? std::numeric_limits<double>::infinity() : M[2*idx2];

  double numerator = 2*cfactor * (v.segment<2>(2*idx2) - v.segment<2>(2*idx1) ).dot(nhat);
  double denom1 = 1+m1/m2;
  double denom2 = m2/m1 + 1;

  if(!scene.isFixed(idx1))
    v.segment<2>(2*idx1) += numerator/denom1 * nhat;
  if(!scene.isFixed(idx2))
    v.segment<2>(2*idx2) -= numerator/denom2 * nhat;
}

bool ExplicitEuler::stepScene( TwoDScene& scene, scalar dt )
{
    VectorXs& x = scene.getX();
    VectorXs& v = scene.getV();
    const VectorXs& m = scene.getM();
    // if( scene.isFixed(i) )  // Determine if the ith particle is fixed

    int num_particles = scene.getNumParticles();
    VectorXs forces(num_particles * 2);
    VectorXs dx(num_particles * 2);
    VectorXs dv(num_particles * 2);
    for (int i = 0; i < num_particles * 2; i++){
      forces(i) = 0.0;
      dx(i) = 0.0;
      dv(i) = 0.0;
    }
    scene.accumulateGradU(forces, dx, dv);//, const VectorXs& dx, const VectorXs& dv )

    for (int i = 0; i < num_particles; i++){
      if (!scene.isFixed(i)){
        int index = 2 * i; // even indeces of vector are x params and odd are y params

        const Vector2s next_velocity(v(index) + dt*forces(index)/m(index),
                                     v(index+1) + dt*forces(index+1)/m(index));
        scene.setVelocity(i, next_velocity);
        
        const Vector2s next_position(x(index) + dt*v(index),
                                     x(index + 1) + dt*v(index+1));
        scene.setPosition(i, next_position);

        scalar kinetic_energy = 0;
        text_file << step_count*dt << "\t" << scene.computeKineticEnergy() << endl;
      }
    }

    step_count++;
    return true;
}

void TwoDSceneSVGRenderer::renderComparisonScene( const std::string& filename, const TwoDScene& otherscene, const std::vector<CollisionInfo> *impulses, const std::vector<CollisionInfo> *otherimpulses, const scalar &eps) const
{
    const VectorXs& x = m_scene.getX();
    const VectorXs& v = m_scene.getV();
    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(filename.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<std::pair<int,int> >& edges = m_scene.getEdges();
    const std::vector<scalar>& edgeradii = m_scene.getEdgeRadii();
    renderShared( file, x, edges, radii, edgeradii, scale, xmin, ymin, xshift, yshift );



    const VectorXs& otherx = otherscene.getX();
    const VectorXs& otherv = otherscene.getV();

    for( int i = 0; i < numparticles; ++i )
    {
        scalar x_resid = (otherx.segment<2>(2*i)-x.segment<2>(2*i)).norm();
        scalar v_resid = (otherv.segment<2>(2*i)-v.segment<2>(2*i)).norm();
        if( x_resid > eps || v_resid > eps )
        {
            Vector2s center;
            center << scale*(x(2*i)-xmin) + xshift, ((scalar)m_h) - scale*(x(2*i+1)-ymin) - yshift;
            renderCircle( file, center, 1.5*scale*radii[i], renderingutils::Color(1.0,0.0,0.0) );
        }
    }

    if(impulses)
    {
        int i=0, j=0;

        // loop over the real impulses
        while(i < (int)impulses->size())
        {
            int curvert = (*impulses)[i].m_idx1;
            CollisionInfo::collisiontype curtype = (*impulses)[i].m_type;
            int curidx2 = (*impulses)[i].m_idx2;

            // all student impulses less than this correct impulse are buggy
            while(j < (int)otherimpulses->size()
                    && (*otherimpulses)[j].m_idx1 < curvert
                    && (*otherimpulses)[j].m_type < curtype
                    && (*otherimpulses)[j].m_idx2 < curidx2)
            {
                renderImpulse( file, otherscene, (*otherimpulses)[j], true);
                j++;
            }

            // check for missed collision
            if( ! (j < (int)otherimpulses->size()
                    && (*otherimpulses)[j].m_idx1 == curvert
                    && (*otherimpulses)[j].m_type == curtype
                    && (*otherimpulses)[j].m_idx2 == curidx2))
            {
                renderImpulse( file, otherscene, (*impulses)[i], false);
            }
            else
            {
                // check for buggy normal
                if( ((*otherimpulses)[j].m_n - (*impulses)[i].m_n).norm() > eps)
                {
                    renderImpulse( file, otherscene, (*impulses)[i], false);
                    renderImpulse( file, otherscene, (*otherimpulses)[j], true);
                }
                j++;
            }

            i++;
        }
        // Any remaining student impulses are buggy
        while(j < (int)otherimpulses->size())
        {
            renderImpulse( file, otherscene, (*otherimpulses)[j], true);
//.........这里部分代码省略.........