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

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;
}

// 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;
}

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;
}