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;
}
void teleop_tracking::combineVertices(const std::vector<teleop_tracking::StlLoader::Facet> &facets,
                                      EigenSTL::vector_Vector3d &vertices,
                                      EigenSTL::vector_Vector3d &face_normals,
                                      std::vector<unsigned> &face_indices)
{
  // The assumption is that these source are empty
  assert(vertices.empty());
  assert(face_normals.empty());
  assert(face_indices.empty());

  EigenSTL::vector_Vector3f float_vector;

  for (std::size_t i = 0; i < facets.size(); ++i)
  {
    const StlLoader::Facet& f = facets[i];

    face_normals.push_back(toEigend(f.normal).normalized());

    unsigned v0 = appendUnique(float_vector, toEigenf(f.vertices[0]));
    unsigned v1 = appendUnique(float_vector, toEigenf(f.vertices[1]));
    unsigned v2 = appendUnique(float_vector, toEigenf(f.vertices[2]));

    // Small triangles should not have edges collapsed together
    assert(v0 != v1);
    assert(v0 != v2);
    assert(v1 != v2);

    face_indices.push_back(v0);
    face_indices.push_back(v1);
    face_indices.push_back(v2);
  }

  // copy the vector of single precision floats to double precision output
  for (std::size_t i = 0; i < float_vector.size(); ++i)
  {
    Eigen::Vector3d v = float_vector[i].cast<double>();
    vertices.push_back(v);
  }
}
bool robot_sphere_representation::RobotSphereRepresentation::saveToSrdfFile(const std::string& srdf_filename) const
{
  genSpheresForAllLinks();

  // Get an SRDFWriter with the data from the current RobotModel
  moveit_setup_assistant::SRDFWriter writer;
  writer.initModel( *robot_model_->getURDF(), *robot_model_->getSRDF() );

  // Delete any existing spheres
  writer.link_sphere_approximations_.clear();

  // Insert generated spheres into SRDFWriter
  EigenSTL::vector_Vector3d centers;
  std::vector<double> radii;
  std::map<std::string, LinkSphereRepresentation*>::const_iterator lsr = links_.begin();
  std::map<std::string, LinkSphereRepresentation*>::const_iterator lsr_end = links_.end();
  for ( ; lsr != lsr_end ; ++lsr )
  {
    centers.clear();
    radii.clear();
    lsr->second->getSpheres(centers, radii);

    if (centers.empty())
    {
      // a link with no geometry is represented by a single radius=0 sphere
      radii.clear();
      radii.push_back(0);
      centers.push_back(Eigen::Vector3d(0,0,0));
    }

    srdf::Model::LinkSpheres lsp;
    lsp.link_ = lsr->first;

    for ( std::size_t i = 0 ; i < centers.size() ; ++i )
    {
      srdf::Model::Sphere sphere;
      sphere.center_x_ = centers[i].x();
      sphere.center_y_ = centers[i].y();
      sphere.center_z_ = centers[i].z();
      sphere.radius_ = radii[i];

      lsp.spheres_.push_back(sphere);
    }

    writer.link_sphere_approximations_.push_back(lsp);
  }

  // write the SRDF to file and return true on success.
  return writer.writeSRDF(srdf_filename);
}
Mesh* createMeshFromAsset(const aiScene* scene, const Eigen::Vector3d &scale, const std::string &resource_name)
{
  if (!scene->HasMeshes())
  {
    logWarn("Assimp reports scene in %s has no meshes", resource_name.c_str());
    return NULL;
  }
  EigenSTL::vector_Vector3d vertices;
  std::vector<unsigned int> triangles;
  extractMeshData(scene, scene->mRootNode, aiMatrix4x4(), scale, vertices, triangles);
  if (vertices.empty())
  {
    logWarn("There are no vertices in the scene %s", resource_name.c_str());
    return NULL;
  }
  if (triangles.empty())
  {
    logWarn("There are no triangles in the scene %s", resource_name.c_str());
    return NULL;
  }
  
  return createMeshFromVertices(vertices, triangles);
}
void collision_detection::StaticDistanceField::initialize(
      const bodies::Body& body,
      double resolution,
      double space_around_body,
      bool save_points)
{
  points_.clear();
  inv_twice_resolution_ = 1.0 / (2.0 * resolution);


  logInform("    create points at res=%f",resolution);
  EigenSTL::vector_Vector3d points;
  determineCollisionPoints(body, resolution, points);

  if (points.empty())
  {
    logWarn("    StaticDistanceField::initialize: No points in body. Using origin.");
    points.push_back(body.getPose().translation());

    if (body.getType() == shapes::MESH)
    {
      const bodies::ConvexMesh& mesh = dynamic_cast<const bodies::ConvexMesh&>(body);
      const EigenSTL::vector_Vector3d& verts = mesh.getVertices();
      logWarn("    StaticDistanceField::initialize: also using %d vertices.", int(verts.size()));

      EigenSTL::vector_Vector3d::const_iterator it = verts.begin();
      EigenSTL::vector_Vector3d::const_iterator it_end = verts.end();
      for ( ; it != it_end ; ++it)
      {
        points.push_back(*it);
      }
    }
  }
  logInform("    StaticDistanceField::initialize: Using %d points.", points.size());

  AABB aabb;
  aabb.add(points);

  logInform("    space_around_body = %f",space_around_body);
  logInform("    DF: min=(%7.3f %7.3f %7.3f)  max=(%7.3f %7.3f %7.3f) (pre-space)",
                              aabb.min_.x(),
                              aabb.min_.y(),
                              aabb.min_.z(),
                              aabb.max_.x(),
                              aabb.max_.y(),
                              aabb.max_.z());

  aabb.min_ -= Eigen::Vector3d(space_around_body, space_around_body, space_around_body);
  aabb.max_ += Eigen::Vector3d(space_around_body, space_around_body, space_around_body);

  logInform("    DF: min=(%7.3f %7.3f %7.3f)  max=(%7.3f %7.3f %7.3f) (pre-adjust)",
                              aabb.min_.x(),
                              aabb.min_.y(),
                              aabb.min_.z(),
                              aabb.max_.x(),
                              aabb.max_.y(),
                              aabb.max_.z());

  aabb.min_.x() = std::floor(aabb.min_.x() / resolution) * resolution;
  aabb.min_.y() = std::floor(aabb.min_.y() / resolution) * resolution;
  aabb.min_.z() = std::floor(aabb.min_.z() / resolution) * resolution;

  logInform("    DF: min=(%7.3f %7.3f %7.3f)  max=(%7.3f %7.3f %7.3f) (post-adjust)",
                              aabb.min_.x(),
                              aabb.min_.y(),
                              aabb.min_.z(),
                              aabb.max_.x(),
                              aabb.max_.y(),
                              aabb.max_.z());

  Eigen::Vector3d size = aabb.max_ - aabb.min_;

  double diagonal = size.norm();

  logInform("    DF: sz=(%7.3f %7.3f %7.3f) cnt=(%d %d %d) diag=%f",
                              size.x(),
                              size.y(),
                              size.z(),
                              int(size.x()/resolution),
                              int(size.y()/resolution),
                              int(size.z()/resolution),
                              diagonal);


  distance_field::PropagationDistanceField df(
                              size.x(),
                              size.y(),
                              size.z(),
                              resolution,
                              aabb.min_.x(),
                              aabb.min_.y(),
                              aabb.min_.z(),
                              diagonal * 2.0,
                              true);
  df.addPointsToField(points);

  DistPosEntry default_entry;
  default_entry.distance_ = diagonal * 2.0;
  default_entry.cell_id_ = -1;

  resize(size.x(),
         size.y(),
         size.z(),
         resolution,
         aabb.min_.x(),
         aabb.min_.y(),
         aabb.min_.z(),
         default_entry);

  logInform("    copy %d points.",
    getNumCells(distance_field::DIM_X) *
    getNumCells(distance_field::DIM_Y) *
    getNumCells(distance_field::DIM_Z));

  int pdf_x,pdf_y,pdf_z;
  int sdf_x,sdf_y,sdf_z;
  Eigen::Vector3d pdf_p, sdf_p;
  df.worldToGrid(aabb.min_.x(), aabb.min_.y(), aabb.min_.z(), pdf_x,pdf_y,pdf_z);
  worldToGrid(aabb.min_.x(), aabb.min_.y(), aabb.min_.z(), sdf_x,sdf_y,sdf_z);
  df.gridToWorld(pdf_x,pdf_y,pdf_z, pdf_p.x(), pdf_p.y(), pdf_p.z());
  gridToWorld(sdf_x,sdf_y,sdf_z, sdf_p.x(), sdf_p.y(), sdf_p.z());

  logInform("    DF: min=(%10.6f %10.6f %10.6f)  quant->%3d %3d %3d  (pdf)",
                              aabb.min_.x(),
                              aabb.min_.y(),
                              aabb.min_.z(),
                              pdf_x,
                              pdf_y,
                              pdf_z);
  logInform("    DF: min=(%10.6f %10.6f %10.6f)  quant<-%3d %3d %3d  (pdf)",
                              pdf_p.x(),
                              pdf_p.y(),
                              pdf_p.z(),
                              pdf_x,
                              pdf_y,
                              pdf_z);
  logInform("    DF: min=(%10.6f %10.6f %10.6f)  quant<-%3d %3d %3d  (sdf)",
                              sdf_p.x(),
                              sdf_p.y(),
                              sdf_p.z(),
                              sdf_x,
                              sdf_y,
                              sdf_z);


  df.worldToGrid(0,0,0, pdf_x,pdf_y,pdf_z);
  worldToGrid(0,0,0, sdf_x,sdf_y,sdf_z);
  df.gridToWorld(pdf_x,pdf_y,pdf_z, pdf_p.x(), pdf_p.y(), pdf_p.z());
  gridToWorld(sdf_x,sdf_y,sdf_z, sdf_p.x(), sdf_p.y(), sdf_p.z());

  logInform("    DF: org=(%10.6f %10.6f %10.6f)  quant->%3d %3d %3d  (pdf)",
                              0.0,
                              0.0,
                              0.0,
                              pdf_x,
                              pdf_y,
                              pdf_z);
  logInform("    DF: org=(%10.6f %10.6f %10.6f)  quant<-%3d %3d %3d  (pdf)",
                              pdf_p.x(),
                              pdf_p.y(),
                              pdf_p.z(),
                              pdf_x,
                              pdf_y,
                              pdf_z);
  logInform("    DF: org=(%10.6f %10.6f %10.6f)  quant<-%3d %3d %3d  (sdf)",
                              sdf_p.x(),
                              sdf_p.y(),
                              sdf_p.z(),
                              sdf_x,
                              sdf_y,
                              sdf_z);


  df.worldToGrid(points[0].x(), points[0].y(), points[0].z(), pdf_x,pdf_y,pdf_z);
  worldToGrid(points[0].x(), points[0].y(), points[0].z(), sdf_x,sdf_y,sdf_z);
  df.gridToWorld(pdf_x,pdf_y,pdf_z, pdf_p.x(), pdf_p.y(), pdf_p.z());
  gridToWorld(sdf_x,sdf_y,sdf_z, sdf_p.x(), sdf_p.y(), sdf_p.z());

  logInform("    DF: p0 =(%10.6f %10.6f %10.6f)  quant->%3d %3d %3d  (pdf)",
                              points[0].x(),
                              points[0].y(),
                              points[0].z(),
                              pdf_x,
                              pdf_y,
                              pdf_z);
  logInform("    DF: p0 =(%10.6f %10.6f %10.6f)  quant<-%3d %3d %3d  (pdf)",
                              pdf_p.x(),
                              pdf_p.y(),
                              pdf_p.z(),
                              pdf_x,
                              pdf_y,
                              pdf_z);
  logInform("    DF: p0 =(%10.6f %10.6f %10.6f)  quant<-%3d %3d %3d  (sdf)",
                              sdf_p.x(),
                              sdf_p.y(),
                              sdf_p.z(),
                              sdf_x,
                              sdf_y,
                              sdf_z);


  for (int z = 0 ; z < df.getZNumCells() ; ++z)
  {
    for (int y = 0 ; y < df.getYNumCells() ; ++y)
    {
      for (int x = 0 ; x < df.getXNumCells() ; ++x)
      {
        DistPosEntry entry;
        double dist = df.getDistance(x, y, z);
        const distance_field::PropDistanceFieldVoxel& voxel = df.getCell(x,y,z);

        if (dist < 0)
        {

          // propogation distance field has a bias of -1*resolution on points inside the object
          if (dist <= -resolution)
          {
            dist += resolution;
          }
          else
          {
            logError("PropagationDistanceField returned distance=%f between 0 and -resolution=%f."
                     "  Did someone fix it?"
                     "  Need to remove workaround from static_distance_field.cpp",
                     dist,-resolution);
            dist = 0.0;
          }
          entry.distance_ = dist;
          entry.cell_id_ = getCellId(
                              voxel.closest_negative_point_.x(),
                              voxel.closest_negative_point_.y(),
                              voxel.closest_negative_point_.z());
        }
        else
        {
          entry.distance_ = dist;
          entry.cell_id_ = getCellId(
                              voxel.closest_point_.x(),
                              voxel.closest_point_.y(),
                              voxel.closest_point_.z());
        }
        setCell(x, y, z, entry);
      }
    }
  }

  if (save_points)
    std::swap(points, points_);
}