C++ ON_NurbsCurve::Evaluate方法代码示例

double
FittingCurve2d::findClosestElementMidPoint (const ON_NurbsCurve &nurbs, const Eigen::Vector2d &pt, double hint)
{
  // evaluate hint
  double param = hint;
  double points[2];
  nurbs.Evaluate (param, 0, 2, points);
  Eigen::Vector2d p (points[0], points[1]);
  Eigen::Vector2d r = p - pt;

  double d_shortest_hint = r.squaredNorm ();
  double d_shortest_elem (DBL_MAX);

  // evaluate elements
  std::vector<double> elements = pcl::on_nurbs::FittingCurve2d::getElementVector (nurbs);
  double seg = 1.0 / (nurbs.Order () - 1);

  for (unsigned i = 0; i < elements.size () - 1; i++)
  {
    double &xi0 = elements[i];
    double &xi1 = elements[i + 1];
    double dxi = xi1 - xi0;

    for (unsigned j = 0; j < nurbs.Order (); j++)
    {
      double xi = xi0 + (seg * j) * dxi;

      nurbs.Evaluate (xi, 0, 2, points);
      p (0) = points[0];
      p (1) = points[1];

      r = p - pt;

      double d = r.squaredNorm ();

      if (d < d_shortest_elem)
      {
        d_shortest_elem = d;
        param = xi;
      }
    }
  }

  if(d_shortest_hint < d_shortest_elem)
    return hint;
  else
    return param;
}

double
FittingCurve::findClosestElementMidPoint (const ON_NurbsCurve &nurbs, const Eigen::Vector3d &pt)
{
  double hint (0.0);
  Eigen::Vector3d p, r;
  std::vector<double> elements = getElementVector (nurbs);
  double points[3];

  double d_shortest (DBL_MAX);

  for (unsigned i = 0; i < elements.size () - 1; i++)
  {
    double xi = elements[i] + 0.5 * (elements[i + 1] - elements[i]);

    nurbs.Evaluate (xi, 0, 3, points);
    p (0) = points[0];
    p (1) = points[1];
    p (2) = points[2];

    r = p - pt;

    double d = r.squaredNorm ();

    if (d < d_shortest)
    {
      d_shortest = d;
      hint = xi;
    }
  }

  return hint;
}

double
FittingCurve::inverseMapping (const ON_NurbsCurve &nurbs, const Eigen::Vector3d &pt, const double &hint, double &error,
                              Eigen::Vector3d &p, Eigen::Vector3d &t, int maxSteps, double accuracy, bool quiet)
{
  //int cp_red = (nurbs.m_order - 2);
  //int ncpj = int (nurbs.m_cv_count - 2 * cp_red);
  double pointAndTangents[6];

  double current, delta;
  Eigen::Vector3d r;
  std::vector<double> elements = getElementVector (nurbs);
  double minU = elements[0];
  double maxU = elements[elements.size () - 1];

  current = hint;

  for (int k = 0; k < maxSteps; k++)
  {

    nurbs.Evaluate (current, 1, 3, pointAndTangents);

    p (0) = pointAndTangents[0];
    p (1) = pointAndTangents[1];
    p (2) = pointAndTangents[2];

    t (0) = pointAndTangents[3];
    t (1) = pointAndTangents[4];
    t (2) = pointAndTangents[5];

    r = p - pt;

    // step width control
    int E = findElement (current, elements);
    double e = elements[E + 1] - elements[E];

    delta = -(0.5 * e) * r.dot (t) / t.norm (); //  A.ldlt().solve(b);

    if (std::fabs (delta) < accuracy)
    {

      error = r.norm ();
      return current;

    }
    else
    {
      current = current + delta;

      if (current < minU)
        current = maxU - (minU - current);
      else if (current > maxU)
        current = minU + (current - maxU);

    }

  }

  error = r.norm ();

  if (!quiet)
  {
    printf ("[FittingCurve::inverseMapping] Warning: Method did not converge (%e %d).\n", accuracy, maxSteps);
    printf ("[FittingCurve::inverseMapping] hint: %f current: %f delta: %f error: %f\n", hint, current, delta, error);
  }

  return current;
}

double
FittingCurve2d::inverseMappingO2 (const ON_NurbsCurve &nurbs, const Eigen::Vector2d &pt, double &error,
                                  Eigen::Vector2d &p, Eigen::Vector2d &t)
{
  if (nurbs.Order () != 2)
    printf ("[FittingCurve2d::inverseMappingO2] Error, order not 2 (polynomial degree 1)\n");

  std::vector<double> elements = getElementVector (nurbs);

  Eigen::Vector2d min_pt;
  double min_param (DBL_MAX);
  double min_dist (DBL_MAX);
  error = DBL_MAX;
  int is_corner (-1);

  for (unsigned i = 0; i < elements.size () - 1; i++)
  {
    Eigen::Vector2d p1;
    nurbs.Evaluate (elements[i], 0, 2, &p1 (0));

    Eigen::Vector2d p2;
    nurbs.Evaluate (elements[i + 1], 0, 2, &p2 (0));

    Eigen::Vector2d d1 (p2 (0) - p1 (0), p2 (1) - p1 (1));
    Eigen::Vector2d d2 (pt (0) - p1 (0), pt (1) - p1 (1));

    double d1_norm = d1.norm ();

    double d0_norm = d1.dot (d2) / d1_norm;
    Eigen::Vector2d d0 = d1 * d0_norm / d1_norm;
    Eigen::Vector2d p0 = p1 + d0;

    if (d0_norm < 0.0)
    {
      double tmp_dist = (p1 - pt).norm ();
      if (tmp_dist < min_dist)
      {
        min_dist = tmp_dist;
        min_pt = p1;
        min_param = elements[i];
        is_corner = i;
      }
    }
    else if (d0_norm >= d1_norm)
    {
      double tmp_dist = (p2 - pt).norm ();
      if (tmp_dist < min_dist)
      {
        min_dist = tmp_dist;
        min_pt = p2;
        min_param = elements[i + 1];
        is_corner = i + 1;
      }
    }
    else
    { // p0 lies on line segment
      double tmp_dist = (p0 - pt).norm ();
      if (tmp_dist < min_dist)
      {
        min_dist = tmp_dist;
        min_pt = p0;
        min_param = elements[i] + (d0_norm / d1_norm) * (elements[i + 1] - elements[i]);
        is_corner = -1;
      }
    }
  }

  if (is_corner >= 0)
  {
    double param1, param2;
    if (is_corner == 0 || is_corner == elements.size () - 1)
    {
      double x0a = elements[0];
      double x0b = elements[elements.size () - 1];
      double xa = elements[1];
      double xb = elements[elements.size () - 2];

      param1 = x0a + 0.5 * (xa - x0a);
      param2 = x0b + 0.5 * (xb - x0b);
    }
    else
    {
      double x0 = elements[is_corner];
      double x1 = elements[is_corner - 1];
      double x2 = elements[is_corner + 1];

      param1 = x0 + 0.5 * (x1 - x0);
      param2 = x0 + 0.5 * (x2 - x0);
    }

    double pt1[4];
    nurbs.Evaluate (param1, 1, 2, pt1);
    Eigen::Vector2d t1 (pt1[2], pt1[3]);
    t1.normalize ();

    double pt2[4];
    nurbs.Evaluate (param2, 1, 2, pt2);
    Eigen::Vector2d t2 (pt2[2], pt2[3]);
    t2.normalize ();

//.........这里部分代码省略.........