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

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

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