diagonalcurves: replace cubic splines with (centripetal) Catmull-Rom splines

See https://en.wikipedia.org/wiki/Centripetal_Catmull%E2%80%93Rom_spline

Fixes #4343
This commit is contained in:
Alberto Griggio
2018-07-26 17:27:57 +02:00
parent 4aaf28760a
commit 6ae287b72b
2 changed files with 155 additions and 11 deletions

View File

@@ -444,6 +444,7 @@ protected:
DiagonalCurveType kind;
void spline_cubic_set ();
void catmull_rom_set();
void NURBS_set ();
public:

View File

@@ -86,7 +86,8 @@ DiagonalCurve::DiagonalCurve (const std::vector<double>& p, int poly_pn)
if (!identity) {
if (kind == DCT_Spline && N > 2) {
spline_cubic_set ();
//spline_cubic_set ();
catmull_rom_set();
} else if (kind == DCT_NURBS && N > 2) {
NURBS_set ();
fillHash();
@@ -270,6 +271,134 @@ void DiagonalCurve::NURBS_set ()
fillDyByDx();
}
/*****************************************************************************
* Catmull Rom Spline
* (https://en.wikipedia.org/wiki/Centripetal_Catmull%E2%80%93Rom_spline)
*****************************************************************************/
namespace {
inline double pow2(double x)
{
return x*x;
}
inline double catmull_rom_tj(double ti,
double xi, double yi,
double xj, double yj)
{
return sqrt(sqrt(pow2(xj-xi) + pow2(yj-yi))) + ti;
}
inline void catmull_rom_spline(int n_points,
double p0_x, double p0_y,
double p1_x, double p1_y,
double p2_x, double p2_y,
double p3_x, double p3_y,
std::vector<double> &res_x,
std::vector<double> &res_y)
{
res_x.reserve(n_points);
res_y.reserve(n_points);
double t0 = 0;
double t1 = catmull_rom_tj(t0, p0_x, p0_y, p1_x, p1_y);
double t2 = catmull_rom_tj(t1, p1_x, p1_y, p2_x, p2_y);
double t3 = catmull_rom_tj(t2, p2_x, p2_y, p3_x, p3_y);
double space = (t2-t1) / n_points;
double t;
int i;
double c, d, A1_x, A1_y, A2_x, A2_y, A3_x, A3_y;
double B1_x, B1_y, B2_x, B2_y, C_x, C_y;
for (i = 0; i < n_points; ++i) {
t = t1 + space * i;
c = (t1 - t)/(t1 - t0);
d = (t - t0)/(t1 - t0);
A1_x = c * p0_x + d * p1_x;
A1_y = c * p0_y + d * p1_y;
c = (t2 - t)/(t2 - t1);
d = (t - t1)/(t2 - t1);
A2_x = c * p1_x + d * p2_x;
A2_y = c * p1_y + d * p2_y;
c = (t3 - t)/(t3 - t2);
d = (t - t2)/(t3 - t2);
A3_x = c * p2_x + d * p3_x;
A3_y = c * p2_y + d * p3_y;
c = (t2 - t)/(t2 - t0);
d = (t - t0)/(t2 - t0);
B1_x = c * A1_x + d * A2_x;
B1_y = c * A1_y + d * A2_y;
c = (t3 - t)/(t3 - t1);
d = (t - t1)/(t3 - t1);
B2_x = c * A2_x + d * A3_x;
B2_y = c * A2_y + d * A3_y;
c = (t2 - t)/(t2 - t1);
d = (t - t1)/(t2 - t1);
C_x = c * B1_x + d * B2_x;
C_y = c * B1_y + d * B2_y;
res_x.push_back(C_x);
res_y.push_back(C_y);
}
}
void catmull_rom_chain(int n_points, int n_cp, double *x, double *y,
std::vector<double> &res_x, std::vector<double> &res_y)
{
static const double epsilon = 1e-5;
double xr = x[1] - x[0];
double yr = y[1] - y[0];
double x_first = x[0] - xr * 0.1;
double y_first = xr > epsilon ? (yr / xr) * (x_first - x[0]) + y[0] : y[0];
xr = x[n_cp-1] - x[n_cp-2];
yr = y[n_cp-1] - x[n_cp-2];
double x_last = x[n_cp-1] + xr * 0.1;
double y_last = xr > epsilon ? (yr / xr) * (x_last - x[0]) + y[0] : y[0];
int segments = n_cp - 1;
int points_segments = n_points / segments;
res_x.reserve(n_points);
res_y.reserve(n_points);
for (int i = 0; i < segments; ++i) {
int n = points_segments + (i == 0 ? n_points % segments : 0);
catmull_rom_spline(
n, i == 0 ? x_first : x[i-1], i == 0 ? y_first : y[i-1],
x[i], y[i], x[i+1], y[i+1],
i == segments-1 ? x_last : x[i+2],
i == segments-1 ? y_last : y[i+2],
res_x, res_y);
}
}
} // namespace
void DiagonalCurve::catmull_rom_set()
{
int n_points = max(ppn * 65, 65000);
poly_x.clear();
poly_y.clear();
catmull_rom_chain(n_points, N, x, y, poly_x, poly_y);
}
/*****************************************************************************/
double DiagonalCurve::getVal (double t) const
{
@@ -300,7 +429,8 @@ double DiagonalCurve::getVal (double t) const
}
case DCT_Linear :
case DCT_Spline : {
// case DCT_Spline :
{
// values under and over the first and last point
if (t > x[N - 1]) {
return y[N - 1];
@@ -324,20 +454,33 @@ double DiagonalCurve::getVal (double t) const
double h = x[k_hi] - x[k_lo];
// linear
if (kind == DCT_Linear) {
// if (kind == DCT_Linear) {
return y[k_lo] + (t - x[k_lo]) * ( y[k_hi] - y[k_lo] ) / h;
}
// spline curve
else { // if (kind==Spline) {
double a = (x[k_hi] - t) / h;
double b = (t - x[k_lo]) / h;
double r = a * y[k_lo] + b * y[k_hi] + ((a * a * a - a) * ypp[k_lo] + (b * b * b - b) * ypp[k_hi]) * (h * h) * 0.1666666666666666666666666666666;
return CLIPD(r);
}
// }
// // spline curve
// else { // if (kind==Spline) {
// double a = (x[k_hi] - t) / h;
// double b = (t - x[k_lo]) / h;
// double r = a * y[k_lo] + b * y[k_hi] + ((a * a * a - a) * ypp[k_lo] + (b * b * b - b) * ypp[k_hi]) * (h * h) * 0.1666666666666666666666666666666;
// return CLIPD(r);
// }
break;
}
case DCT_Spline: {
auto it = std::lower_bound(poly_x.begin(), poly_x.end(), t);
if (it == poly_x.end()) {
return poly_y.back();
}
auto d = it - poly_x.begin();
if (it+1 < poly_x.end() && t - *it > *(it+1) - t) {
++d;
}
return *(poly_y.begin() + d);
break;
}
case DCT_NURBS : {
// get the hash table entry by rounding the value (previously multiplied by "hashSize")
unsigned short int i = (unsigned short int)(t * hashSize);