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