void BezierInterpolator::InterpolateBezier(const QPointF &p1, const QPointF &p2,
                                           const QPointF &p3, const QPointF &p4,
                                           QPolygonF &interpolatedPoints,
                                           unsigned level) const {
  InterpolateBezier(p1.x(), p1.y(), p2.x(), p2.y(), p3.x(), p3.y(), p4.x(),
                    p4.y(), interpolatedPoints, level);
}
Exemplo n.º 2
0
Vertex InterpolateCurve(Curve *c, double u, int derivee)
{
  if(c->Num < 0) {
    Curve *C0 = FindCurve(-c->Num);
    if(!C0){
      Msg::Error("Unknown curve %d", -c->Num);
      return Vertex(0., 0., 0.);
    }
    return InterpolateCurve(C0, C0->ubeg + (C0->uend - C0->ubeg) * (1. - u), derivee);
  }

  Vertex V;

  if(derivee==1) {
    // switch (c->Typ) {
    // case MSH_SEGM_BSPLN:
    // case MSH_SEGM_BEZIER:
    //   V = InterpolateUBS(c, u, 1);
    //   V.u = u;
    //   break;
    // default :
      double eps1 = (u == 0) ? 0 : 1.e-5;
      double eps2 = (u == 1) ? 0 : 1.e-5;
      Vertex D[2];
      D[0] = InterpolateCurve(c, u - eps1, 0);
      D[1] = InterpolateCurve(c, u + eps2, 0);
      V.Pos.X = (D[1].Pos.X - D[0].Pos.X) / (eps1 + eps2);
      V.Pos.Y = (D[1].Pos.Y - D[0].Pos.Y) / (eps1 + eps2);
      V.Pos.Z = (D[1].Pos.Z - D[0].Pos.Z) / (eps1 + eps2);
      V.u = u;
    //   break;
    // }
    return V;
  }

  if(derivee==2) {
    switch (c->Typ) {
    case MSH_SEGM_BSPLN:
      V = InterpolateUBS(c, u, 2);
      V.u = u;
      break;
    case MSH_SEGM_BEZIER:
      V = InterpolateBezier(c, u, 2);
      V.u = u;
      break;
    default :
      double eps1 = (u == 0) ? 0 : 1.e-5;
      double eps2 = (u == 1) ? 0 : 1.e-5;
      Vertex D[2];
      D[0] = InterpolateCurve(c, u - eps1, 1);
      D[1] = InterpolateCurve(c, u + eps2, 1);
      V.Pos.X = (D[1].Pos.X - D[0].Pos.X) / (eps1 + eps2);
      V.Pos.Y = (D[1].Pos.Y - D[0].Pos.Y) / (eps1 + eps2);
      V.Pos.Z = (D[1].Pos.Z - D[0].Pos.Z) / (eps1 + eps2);
      V.u = u;
      break;
    }
    return V;
  }

  int N, i;
  Vertex *v[5];
  double theta, t1, t2, t;
  Vertex temp1, temp2;

  switch (c->Typ) {

  case MSH_SEGM_LINE:
#if defined(HAVE_BFGS)
    //    printf("MSH_SEGM_LINE\n");
#endif
    N = List_Nbr(c->Control_Points);
    i = (int)((double)(N - 1) * u);
    while(i >= N - 1)
      i--;
    while(i < 0)
      i++;
    t1 = (double)(i) / (double)(N - 1);
    t2 = (double)(i + 1) / (double)(N - 1);
    t = (u - t1) / (t2 - t1);
    List_Read(c->Control_Points, i, &v[1]);
    List_Read(c->Control_Points, i + 1, &v[2]);
    if(!c->geometry){
      V.Pos.X = v[1]->Pos.X + t * (v[2]->Pos.X - v[1]->Pos.X);
      V.Pos.Y = v[1]->Pos.Y + t * (v[2]->Pos.Y - v[1]->Pos.Y);
      V.Pos.Z = v[1]->Pos.Z + t * (v[2]->Pos.Z - v[1]->Pos.Z);
      V.w = (1. - t) * v[1]->w + t * v[2]->w;
      V.lc = (1. - t) * v[1]->lc + t * v[2]->lc;
    }
    else{
      SPoint2 p = v[1]->pntOnGeometry +  (v[2]->pntOnGeometry - v[1]->pntOnGeometry) * t;
      SPoint3 pp = c->geometry->point(p);
      V.Pos.X = pp.x();
      V.Pos.Y = pp.y();
      V.Pos.Z = pp.z();
    }
    break;

  case MSH_SEGM_CIRC:
  case MSH_SEGM_CIRC_INV:
  case MSH_SEGM_ELLI:
  case MSH_SEGM_ELLI_INV:
    if(c->Typ == MSH_SEGM_CIRC_INV || c->Typ == MSH_SEGM_ELLI_INV) {
      V.u = 1. - u;
      u = V.u;
    }
    theta = c->Circle.t1 - (c->Circle.t1 - c->Circle.t2) * u;
    theta -= c->Circle.incl; // for ellipses
    V.Pos.X =
      c->Circle.f1 * cos(theta) * cos(c->Circle.incl) -
      c->Circle.f2 * sin(theta) * sin(c->Circle.incl);
    V.Pos.Y =
      c->Circle.f1 * cos(theta) * sin(c->Circle.incl) +
      c->Circle.f2 * sin(theta) * cos(c->Circle.incl);
    V.Pos.Z = 0.0;
    Projette(&V, c->Circle.invmat);
    List_Read(c->Control_Points, 1, &v[0]);
    V.Pos.X += v[0]->Pos.X;
    V.Pos.Y += v[0]->Pos.Y;
    V.Pos.Z += v[0]->Pos.Z;
    V.w = (1. - u) * c->beg->w + u * c->end->w;
    V.lc = (1. - u) * c->beg->lc + u * c->end->lc;
    break;

  case MSH_SEGM_BSPLN:
    V = InterpolateUBS(c, u, 0);
    break;
  case MSH_SEGM_BEZIER:
    V = InterpolateBezier(c, u, 0);
    break;

  case MSH_SEGM_NURBS:
    V = InterpolateNurbs(c, u, 0);
    break;

  case MSH_SEGM_SPLN:
    N = List_Nbr(c->Control_Points);
    i = (int)((double)(N - 1) * u);
    if(i < 0)
      i = 0;
    if(i >= N - 1)
      i = N - 2;
    t1 = (double)(i) / (double)(N - 1);
    t2 = (double)(i + 1) / (double)(N - 1);
    t = (u - t1) / (t2 - t1);
    List_Read(c->Control_Points, i, &v[1]);
    List_Read(c->Control_Points, i + 1, &v[2]);
    if(!i) {
      if(c->beg == c->end){
        List_Read(c->Control_Points, N - 2, &v[0]);
      }
      else{
        v[0] = &temp1;
        v[0]->Pos.X = 2. * v[1]->Pos.X - v[2]->Pos.X;
        v[0]->Pos.Y = 2. * v[1]->Pos.Y - v[2]->Pos.Y;
        v[0]->Pos.Z = 2. * v[1]->Pos.Z - v[2]->Pos.Z;
        v[0]->pntOnGeometry = v[1]->pntOnGeometry * 2. - v[2]->pntOnGeometry;
      }
    }
    else {
      List_Read(c->Control_Points, i - 1, &v[0]);
    }
    if(i == N - 2) {
      if(c->beg == c->end){
        List_Read(c->Control_Points, 1, &v[3]);
      }
      else{
        v[3] = &temp2;
        v[3]->Pos.X = 2. * v[2]->Pos.X - v[1]->Pos.X;
        v[3]->Pos.Y = 2. * v[2]->Pos.Y - v[1]->Pos.Y;
        v[3]->Pos.Z = 2. * v[2]->Pos.Z - v[1]->Pos.Z;
        v[3]->pntOnGeometry = v[2]->pntOnGeometry * 2. - v[1]->pntOnGeometry;
      }
    }
    else {
      List_Read(c->Control_Points, i + 2, &v[3]);
    }
    if(c->geometry){
      SPoint2 pp = InterpolateCubicSpline(v, t, c->mat, t1, t2,c->geometry,0);
      SPoint3 pt = c->geometry->point(pp);
      V.Pos.X = pt.x();
      V.Pos.Y = pt.y();
      V.Pos.Z = pt.z();
    }
    else
      V = InterpolateCubicSpline(v, t, c->mat, 0, t1, t2);
    break;

  case MSH_SEGM_BND_LAYER:
    Msg::Debug("Cannot interpolate boundary layer curve");
    break;

  case MSH_SEGM_DISCRETE:
    Msg::Debug("Cannot interpolate discrete curve");
    break;

  case MSH_SEGM_COMPOUND:
    Msg::Debug("Cannot interpolate compound curve");
    break;

  default:
    Msg::Error("Unknown curve type in interpolation");
    break;
  }
  V.u = u;
  return V;
}
// InterpolateBezier - interpolates points with bezier curve.
// Algorithm is based on article "Adaptive Subdivision of Bezier Curves" by
// Maxim Shemanarev.
// http://www.antigrain.com/research/adaptive_bezier/index.html
void BezierInterpolator::InterpolateBezier(double x1, double y1,
                                           double x2, double y2,
                                           double x3, double y3,
                                           double x4, double y4,
                                           QPolygonF &interpolatedPoints,
                                           unsigned level) const {
  if(level > curveRecursionLimit) {
    return;
  }

  // Calculate all the mid-points of the line segments
  double x12   = (x1 + x2) / 2;
  double y12   = (y1 + y2) / 2;
  double x23   = (x2 + x3) / 2;
  double y23   = (y2 + y3) / 2;
  double x34   = (x3 + x4) / 2;
  double y34   = (y3 + y4) / 2;
  double x123  = (x12 + x23) / 2;
  double y123  = (y12 + y23) / 2;
  double x234  = (x23 + x34) / 2;
  double y234  = (y23 + y34) / 2;
  double x1234 = (x123 + x234) / 2;
  double y1234 = (y123 + y234) / 2;

  if(level > 0) {
    // Enforce subdivision first time

    // Try to approximate the full cubic curve by a single straight line
    double dx = x4-x1;
    double dy = y4-y1;

    double d2 = qAbs(((x2 - x4) * dy - (y2 - y4) * dx));
    double d3 = qAbs(((x3 - x4) * dy - (y3 - y4) * dx));

    double da1, da2;

    if(d2 > curveCollinearityEpsilon && d3 > curveCollinearityEpsilon) {
      // Regular care
      if((d2 + d3)*(d2 + d3) <= DistanceTolerance * (dx*dx + dy*dy)) {
        // If the curvature doesn't exceed the distance_tolerance value
        // we tend to finish subdivisions.
        if(AngleTolerance < curveAngleToleranceEpsilon) {
          interpolatedPoints.push_back(QPointF(x1234, y1234));
          return;
        }

        // Angle & Cusp Condition
        double a23 = qAtan2(y3 - y2, x3 - x2);
        da1 = fabs(a23 - qAtan2(y2 - y1, x2 - x1));
        da2 = fabs(qAtan2(y4 - y3, x4 - x3) - a23);
        if(da1 >= M_PI) da1 = 2*M_PI - da1;
        if(da2 >= M_PI) da2 = 2*M_PI - da2;

        if(da1 + da2 < AngleTolerance) {
          // Finally we can stop the recursion
          interpolatedPoints.push_back(QPointF(x1234, y1234));
          return;
        }

        if(CuspLimit != 0.0) {
          if(da1 > CuspLimit) {
            interpolatedPoints.push_back(QPointF(x2, y2));
            return;
          }

          if(da2 > CuspLimit) {
            interpolatedPoints.push_back(QPointF(x3, y3));
            return;
          }
        }
      }
    } else {
      if(d2 > curveCollinearityEpsilon) {
        // p1,p3,p4 are collinear, p2 is considerable
        if(d2 * d2 <= DistanceTolerance * (dx*dx + dy*dy)) {
          if(AngleTolerance < curveAngleToleranceEpsilon) {
            interpolatedPoints.push_back(QPointF(x1234, y1234));
            return;
          }

          // Angle Condition
          da1 = fabs(qAtan2(y3 - y2, x3 - x2) - qAtan2(y2 - y1, x2 - x1));
          if(da1 >= M_PI)
            da1 = 2*M_PI - da1;

          if(da1 < AngleTolerance) {
            interpolatedPoints.push_back(QPointF(x2, y2));
            interpolatedPoints.push_back(QPointF(x3, y3));
            return;
          }

          if(CuspLimit != 0.0) {
            if(da1 > CuspLimit) {
              interpolatedPoints.push_back(QPointF(x2, y2));
              return;
            }
          }
        }
      } else if(d3 > curveCollinearityEpsilon) {
        // p1,p2,p4 are collinear, p3 is considerable
        if(d3 * d3 <= DistanceTolerance * (dx*dx + dy*dy)) {
          if(AngleTolerance < curveAngleToleranceEpsilon) {
            interpolatedPoints.push_back(QPointF(x1234, y1234));
            return;
          }

          // Angle Condition
          da1 = fabs(qAtan2(y4 - y3, x4 - x3) - qAtan2(y3 - y2, x3 - x2));
          if(da1 >= M_PI) da1 = 2*M_PI - da1;

          if(da1 < AngleTolerance) {
            interpolatedPoints.push_back(QPointF(x2, y2));
            interpolatedPoints.push_back(QPointF(x3, y3));
            return;
          }

          if(CuspLimit != 0.0) {
            if(da1 > CuspLimit) {
              interpolatedPoints.push_back(QPointF(x3, y3));
              return;
            }
          }
        }
      } else {
        // Collinear case
        dx = x1234 - (x1 + x4) / 2;
        dy = y1234 - (y1 + y4) / 2;
        if(dx*dx + dy*dy <= DistanceTolerance) {
          interpolatedPoints.push_back(QPointF(x1234, y1234));
          return;
        }
      }
    }
  }

  // Continue subdivision
  InterpolateBezier(x1, y1, x12, y12, x123, y123, x1234, y1234,
                    interpolatedPoints, level + 1);
  InterpolateBezier(x1234, y1234, x234, y234, x34, y34, x4, y4,
                    interpolatedPoints, level + 1);
}