void CollisionWorldDistanceField::updateDistanceObject(const std::string& id,
                                                       boost::shared_ptr<CollisionWorldDistanceField::DistanceFieldCacheEntry>& dfce,
                                                       EigenSTL::vector_Vector3d& add_points,
                                                       EigenSTL::vector_Vector3d& subtract_points)
{
  std::map<std::string, std::vector<PosedBodyPointDecompositionPtr> >::iterator cur_it = dfce->posed_body_point_decompositions_.find(id);
  if(cur_it != dfce->posed_body_point_decompositions_.end()) {
    for(unsigned int i = 0; i < cur_it->second.size(); i++) {
      subtract_points.insert(subtract_points.end(),
                             cur_it->second[i]->getCollisionPoints().begin(),
                             cur_it->second[i]->getCollisionPoints().end());
    }
  }
  ObjectConstPtr object = getObject(id);
  if(object) {
    std::vector<PosedBodyPointDecompositionPtr> shape_points;
    for(unsigned int i = 0; i < object->shapes_.size(); i++) {
      
      BodyDecompositionConstPtr bd = getBodyDecompositionCacheEntry(object->shapes_[i],
                                                                    resolution_);
      
      shape_points.push_back(boost::make_shared<PosedBodyPointDecomposition>(bd, object->shape_poses_[i]));
      add_points.insert(add_points.end(),
                        shape_points.back()->getCollisionPoints().begin(),
                        shape_points.back()->getCollisionPoints().end());
    }
    dfce->posed_body_point_decompositions_[id] = shape_points;
  } else {
    dfce->posed_body_point_decompositions_.erase(id);
  }
}
Пример #2
0
void mesh_core::Plane::leastSquaresGeneral(
      const EigenSTL::vector_Vector3d& points,
      Eigen::Vector3d* average)
{
  if (points.empty())
  {
    normal_ = Eigen::Vector3d(0,0,1);
    d_ = 0;
    if (average)
      *average = Eigen::Vector3d::Zero();
    return;
  }

  // find c, the average of the points
  Eigen::Vector3d c;
  c.setZero();

  EigenSTL::vector_Vector3d::const_iterator p = points.begin();
  EigenSTL::vector_Vector3d::const_iterator end = points.end();
  for ( ; p != end ; ++p)
    c += *p;

  c *= 1.0/double(points.size());

  // Find the matrix
  Eigen::Matrix3d m;
  m.setZero();

  p = points.begin();
  for ( ; p != end ; ++p)
  {
    Eigen::Vector3d cp = *p - c;
    m(0,0) += cp.x() * cp.x();
    m(1,0) += cp.x() * cp.y();
    m(2,0) += cp.x() * cp.z();
    m(1,1) += cp.y() * cp.y();
    m(2,1) += cp.y() * cp.z();
    m(2,2) += cp.z() * cp.z();
  }
  m(0,1) = m(1,0);
  m(0,2) = m(2,0);
  m(1,2) = m(2,1);

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(m);
  if (eigensolver.info() == Eigen::Success)
  {
    normal_ = eigensolver.eigenvectors().col(0);
    normal_.normalize();
  }
  else
  {
    normal_ = Eigen::Vector3d(0,0,1);
  }

  d_ = -c.dot(normal_);

  if (average)
    *average = c;
}
Пример #3
0
void mesh_core::generateAABB(
      const EigenSTL::vector_Vector3d& points,
      Eigen::Vector3d& min,
      Eigen::Vector3d& max)
{
  min = Eigen::Vector3d(std::numeric_limits<double>::max(),
                        std::numeric_limits<double>::max(),
                        std::numeric_limits<double>::max());
  max = Eigen::Vector3d(-std::numeric_limits<double>::max(),
                        -std::numeric_limits<double>::max(),
                        -std::numeric_limits<double>::max());

  EigenSTL::vector_Vector3d::const_iterator it = points.begin();
  EigenSTL::vector_Vector3d::const_iterator end = points.end();
  for ( ; it != end ; ++it)
  {
    min = min.array().min(it->array());
    max = max.array().max(it->array());
  }
}
Пример #4
0
void mesh_core::Plane::leastSquaresFast(
      const EigenSTL::vector_Vector3d& points,
      Eigen::Vector3d* average)
{
  Eigen::Matrix3d m;
  Eigen::Vector3d b;
  Eigen::Vector3d c;

  m.setZero();
  b.setZero();
  c.setZero();

  EigenSTL::vector_Vector3d::const_iterator p = points.begin();
  EigenSTL::vector_Vector3d::const_iterator end = points.end();
  for ( ; p != end ; ++p)
  {
    m(0,0) += p->x() * p->x();
    m(1,0) += p->x() * p->y();
    m(2,0) += p->x();
    m(1,1) += p->y() * p->y();
    m(2,1) += p->y();
    b(0) += p->x() * p->z();
    b(1) += p->y() * p->z();
    b(2) += p->z();
    c += *p;
  }
  m(0,1) = m(1,0);
  m(0,2) = m(2,0);
  m(1,2) = m(2,1);
  m(2,2) = double(points.size());
  c *= 1.0/double(points.size());

  normal_ = m.colPivHouseholderQr().solve(b);
  if (normal_.squaredNorm() > std::numeric_limits<double>::epsilon())
    normal_.normalize();

  d_ = -c.dot(normal_);

  if (average)
    *average = c;
}