diff options
Diffstat (limited to 'source/blender/blenlib/BLI_even_spline.hh')
| -rw-r--r-- | source/blender/blenlib/BLI_even_spline.hh | 733 |
1 files changed, 733 insertions, 0 deletions
diff --git a/source/blender/blenlib/BLI_even_spline.hh b/source/blender/blenlib/BLI_even_spline.hh new file mode 100644 index 00000000000..4b80df0202c --- /dev/null +++ b/source/blender/blenlib/BLI_even_spline.hh @@ -0,0 +1,733 @@ +#pragma once + +#include "BLI_compiler_attrs.h" +#include "BLI_compiler_compat.h" + +#include "BLI_math.h" +#include "BLI_math_vec_types.hh" +#include "BLI_vector.hh" + +#include <cstdio> +#include <utility> + +//#define FINITE_DIFF + +/* + * Arc length parameterized spline library. + */ +namespace blender { +/* + Abstract curve interface. + +template<typename Float> class Curve { + using Vector = vec_base<Float, 2>; + + public: + Float length; + + Vector evaluate(Float s); + Vector derivative(Float s); + Vector derivative2(Float s); + Float curvature(float s); + + void update(); +}; +*/ + +/* +comment: Reduce algebra script; + +on factor; +off period; + +procedure bez(a, b); + a + (b - a) * t; + +lin := bez(k1, k2); +quad := bez(lin, sub(k2=k3, k1=k2, lin)); + +cubic := bez(quad, sub(k3=k4, k2=k3, k1=k2, quad)); +dcubic := df(cubic, t); +icubic := int(cubic, t); + +x1 := 0; +y1 := 0; + +dx := sub(k1=x1, k2=x2, k3=x3, k4=x4, dcubic); +dy := sub(k1=y1, k2=y2, k3=y3, k4=y4, dcubic); +darc := sqrt(dx**2 + dy**2); + +arcstep := darc*dt + 0.5*df(darc, t)*dt*dt; + +d2x := df(dx / darc, t); +d2y := df(dy / darc, t); + +gentran +begin +declare << +x1,x2,x3,x4 : float; +y1,y2,y3,y4 : float; +dt,t : float; +>>; +return eval(arcstep) +end; + +on fort; +cubic; +dcubic; +icubic; +arcstep; +d2x; +d2y; +off fort; + +*/ +template<typename Float, int axes = 2, int table_size = 512> class CubicBezier { + using Vector = vec_base<Float, axes>; + + public: + Vector ps[4]; + + CubicBezier(Vector a, Vector b, Vector c, Vector d) + { + ps[0] = a; + ps[1] = b; + ps[2] = c; + ps[3] = d; + + deleted = false; + _arc_to_t = new Float[table_size]; + } + + ~CubicBezier() + { + deleted = true; + + if (_arc_to_t) { + delete[] _arc_to_t; + _arc_to_t = nullptr; + } + } + + CubicBezier() + { + deleted = false; + _arc_to_t = new Float[table_size]; + } + + CubicBezier(const CubicBezier &b) + { + _arc_to_t = new Float[table_size]; + *this = b; + deleted = false; + } + + CubicBezier &operator=(const CubicBezier &b) + { + ps[0] = b.ps[0]; + ps[1] = b.ps[1]; + ps[2] = b.ps[2]; + ps[3] = b.ps[3]; + + length = b.length; + + if (!_arc_to_t) { + _arc_to_t = new Float[table_size]; + } + + if (b._arc_to_t) { + for (int i = 0; i < table_size; i++) { + _arc_to_t[i] = b._arc_to_t[i]; + } + } + + return *this; + } + +#if 1 + CubicBezier(CubicBezier &&b) + { + *this = b; + } + + CubicBezier &operator=(CubicBezier &&b) + { + ps[0] = b.ps[0]; + ps[1] = b.ps[1]; + ps[2] = b.ps[2]; + ps[3] = b.ps[3]; + + length = b.length; + + if (b._arc_to_t) { + _arc_to_t = std::move(b._arc_to_t); + b._arc_to_t = nullptr; + } + else { + _arc_to_t = new Float[table_size]; + } + + return *this; + } +#endif + + Float length; + + void update() + { + Float t = 0.0, dt = 1.0 / (Float)table_size; + Float s = 0.0; + + if (!_arc_to_t) { + _arc_to_t = new Float[table_size]; + } + + auto table = _arc_to_t; + + for (int i = 0; i < table_size; i++) { + table[i] = -1.0; + } + + length = 0.0; + + for (int i = 0; i < table_size; i++, t += dt) { + Float dlen = 0.0; + for (int j = 0; j < axes; j++) { + float dv = dcubic(ps[0][j], ps[1][j], ps[2][j], ps[3][j], t); + + dlen += dv * dv; + } + + dlen = sqrt(dlen) * dt; + + length += dlen; + } + + const int samples = table_size; + dt = 1.0 / (Float)samples; + + t = 0.0; + s = 0.0; + + for (int i = 0; i < samples; i++, t += dt) { + Float dlen = 0.0; + for (int j = 0; j < axes; j++) { + float dv = dcubic(ps[0][j], ps[1][j], ps[2][j], ps[3][j], t); + + dlen += dv * dv; + } + + dlen = sqrt(dlen) * dt; + + int j = (int)((s / length) * (Float)table_size * 0.999999); + j = min_ii(j, table_size - 1); + + table[j] = t; + + s += dlen; + } + + table[0] = 0.0; + table[table_size - 1] = 1.0; + +#if 1 + /* Interpolate gaps in table. */ + for (int i = 0; i < table_size - 1; i++) { + if (table[i] == -1.0 || table[i + 1] != -1.0) { + continue; + } + + int i1 = i; + int i2 = i + 1; + + while (table[i2] == -1.0) { + i2++; + } + + int start = table[i1]; + int end = table[i2]; + Float dt2 = 1.0 / (i2 - i1); + + for (int j = i1 + 1; j < i2; j++) { + Float factor = (Float)(j - i1) * dt2; + table[j] = start + (end - start) * factor; + } + + i = i2 - 1; + } + +# if 0 + for (int i = 0; i < table_size; i++) { + printf("%.3f ", table[i]); + } + printf("\n\n"); +# endif +#endif + } + + inline Vector evaluate(Float s) + { + Float t = arc_to_t(s); + Vector r; + + for (int i = 0; i < axes; i++) { + r[i] = cubic(ps[0][i], ps[1][i], ps[2][i], ps[3][i], t); + } + + return r; + } + + Vector derivative(Float s, bool exact = true) + { + Float t = arc_to_t(s); + Vector r; + + for (int i = 0; i < axes; i++) { + r[i] = dcubic(ps[0][i], ps[1][i], ps[2][i], ps[3][i], t) * length; + } + + /* Real arc length parameterized tangent has unit length. */ + if (exact) { + Float len = sqrt(_dot(r, r)); + + if (len > 0.00001) { + r = r / len; + } + } + + return r; + } + + Vector derivative2(Float s) + { +#ifdef FINITE_DIFF + const Float df = 0.0005; + Float s1, s2; + + if (s >= 1.0 - df) { + s1 = s - df; + s2 = s; + } + else { + s1 = s; + s2 = s + df; + } + + Vector a = derivative(s1); + Vector b = derivative(s2); + + return (b - a) / df; +#else + Float t = arc_to_t(s); + Vector r; + + Float dx = dcubic(ps[0][0], ps[1][0], ps[2][0], ps[3][0], t); + Float d2x = d2cubic(ps[0][0], ps[1][0], ps[2][0], ps[3][0], t); + Float dy = dcubic(ps[0][1], ps[1][1], ps[2][1], ps[3][1], t); + Float d2y = d2cubic(ps[0][1], ps[1][1], ps[2][1], ps[3][1], t); + + /* + comment: arc length second derivative; + + operator x, y, z, dx, dy, dz, d2x, d2y, d2z; + forall t let df(x(t), t) = dx(t); + forall t let df(y(t), t) = dy(t); + forall t let df(z(t), t) = dz(t); + forall t let df(dx(t), t) = d2x(t); + forall t let df(dy(t), t) = d2y(t); + forall t let df(dz(t), t) = d2z(t); + + comment: arc length first derivative is just the normalized tangent; + + comment: 2d case; + + dlen := sqrt(df(x(t), t)**2 + df(y(t), t)**2); + + df(df(x(t), t) / dlen, t); + df(df(y(t), t) / dlen, t); + + comment: 3d case; + + dlen := sqrt(df(x(t), t)**2 + df(y(t), t)**2 + df(z(t), t)**2); + + comment: final derivatives; + + df(df(x(t), t) / dlen, t); + df(df(y(t), t) / dlen, t); + df(df(z(t), t) / dlen, t); + */ + if constexpr (axes == 2) { + /* Basically the 2d perpidicular normalized tangent multiplied by the curvature. */ + + Float div = sqrt(dx * dx + dy * dy) * (dx * dx + dy * dy); + + r[0] = ((d2x * dy - d2y * dx) * dy) / div; + r[1] = (-(d2x * dy - d2y * dx) * dx) / div; + } + else if constexpr (axes == 3) { + Float dz = dcubic(ps[0][2], ps[1][2], ps[2][2], ps[3][2], t); + Float d2z = d2cubic(ps[0][2], ps[1][2], ps[2][2], ps[3][2], t); + + Float div = sqrt(dx * dx + dy * dy + dz * dz) * (dy * dy + dz * dz + dx * dx); + + r[0] = (d2x * dy * dy + d2x * dz * dz - d2y * dx * dy - d2z * dx * dz) / div; + r[1] = (-(d2x * dx * dy - d2y * dx * dx - d2y * dz * dz + d2z * dy * dz)) / div; + r[2] = (-(d2x * dx * dz + d2y * dy * dz - d2z * dx * dx - d2z * dy * dy)) / div; + } + else { + for (int i = 0; i < axes; i++) { + r[i] = d2cubic(ps[0][i], ps[1][i], ps[2][i], ps[3][i], t) * length; + } + } + + return r; +#endif + } + + Float curvature(Float s) + { + Vector dv2 = derivative2(s); + + if constexpr (axes == 2) { + Vector dv = derivative(s, true); + + /* Calculate signed curvature. Remember that dv is normalized. */ + return dv[0] * dv2[1] - dv[1] * dv2[0]; + } + + return sqrt(_dot(dv2, dv2)); + } + + private: + Float *_arc_to_t; + bool deleted = false; + + Float cubic(Float k1, Float k2, Float k3, Float k4, Float t) + { + return -(((3.0 * (t - 1.0) * k3 - k4 * t) * t - 3.0 * (t - 1.0) * (t - 1.0) * k2) * t + + (t - 1) * (t - 1) * (t - 1) * k1); + } + + Float dcubic(Float k1, Float k2, Float k3, Float k4, Float t) + { + return -3.0 * ((t - 1.0) * (t - 1.0) * k1 - k4 * t * t + (3.0 * t - 2.0) * k3 * t - + (3.0 * t - 1.0) * (t - 1.0) * k2); + } + + Float d2cubic(Float k1, Float k2, Float k3, Float k4, Float t) + { + return -6.0 * (k1 * t - k1 - 3.0 * k2 * t + 2.0 * k2 + 3.0 * k3 * t - k3 - k4 * t); + } + + Float _dot(Vector a, Vector b) + { + Float sum = 0.0; + + for (int i = 0; i < axes; i++) { + sum += a[i] * b[i]; + } + + return sum; + } + + Float clamp_s(Float s) + { + s = s < 0.0 ? 0.0 : s; + s = s >= length ? length * 0.999999 : s; + + return s; + } + + Float arc_to_t(Float s) + { + if (length == 0.0) { + return 0.0; + } + + s = clamp_s(s); + + Float t = s * (Float)(table_size - 1) / length; + + int i1 = floorf(t); + int i2 = min_ii(i1 + 1, table_size - 1); + + t -= (Float)i1; + + Float s1 = _arc_to_t[i1]; + Float s2 = _arc_to_t[i2]; + + return s1 + (s2 - s1) * t; + } +}; + +template<typename Float, int axes = 2> class BezierSpline { + using Vector = vec_base<Float, axes>; + struct Segment { + CubicBezier<Float, axes> bezier; + Float start = 0.0; + + Segment(const CubicBezier<Float> &bez) + { + bezier = bez; + } + + Segment(const Segment &b) + { + *this = b; + } + + Segment &operator=(const Segment &b) + { + bezier = b.bezier; + start = b.start; + + return *this; + } + + Segment() + { + } + }; + + public: + Float length = 0.0; + bool deleted = false; + blender::Vector<Segment> segments; + + void clear() + { + segments.clear(); + } + + BezierSpline() + { + } + + ~BezierSpline() + { + deleted = true; + } + + void add(CubicBezier<Float, axes> &bez) + { + need_update = true; + + Segment seg; + + seg.bezier = bez; + segments.append(seg); + + update(); + } + + void update() + { + need_update = false; + + length = 0.0; + for (Segment &seg : segments) { + seg.start = length; + length += seg.bezier.length; + } + } + + inline Vector evaluate(Float s) + { + if (s == 0.0) { + return segments[0].bezier.ps[0]; + } + + if (s >= length) { + return segments[segments.size() - 1].bezier.ps[3]; + } + + Segment *seg = get_segment(s); + + return seg->bezier.evaluate(s - seg->start); + } + + Vector derivative(Float s, bool exact = true) + { + if (segments.size() == 0) { + return Vector(); + } + + s = clamp_s(s); + Segment *seg = get_segment(s); + + return seg->bezier.derivative(s - seg->start, exact); + } + + Vector derivative2(Float s) + { + if (segments.size() == 0) { + return Vector(); + } + + s = clamp_s(s); + Segment *seg = get_segment(s); + + return seg->bezier.derivative2(s - seg->start); + } + + Float curvature(Float s) + { + if (segments.size() == 0) { + return 0.0; + } + + s = clamp_s(s); + Segment *seg = get_segment(s); + + return seg->bezier.curvature(s - seg->start); + } + + /* Find the closest point on the spline. Uses a bisecting root finding approach. + * Note: in thoery we could split the spline into quadratic segments and solve + * for the closest point directy. + */ + Vector closest_point(const Vector p, Float &r_s, Vector &r_tan, Float &r_dis) + { + if (segments.size() == 0) { + return Vector(); + } + + const int steps = 12; + Float s = 0.0, ds = length / steps; + Float mindis = FLT_MAX; + Vector minp; + Float mins = 0.0; + bool found = false; + + Vector lastdv, lastp; + Vector b, dvb; + + for (int i = 0; i < steps + 1; i++, s += ds, lastp = b, lastdv = dvb) { + b = evaluate(s); + dvb = derivative(s, false); /* We don't need real normalized derivative here. */ + + if (i == 0) { + continue; + } + + Vector dva = lastdv; + Vector a = lastp; + + Vector vec1 = a - p; + Vector vec2 = b - p; + + Float sign1 = _dot(vec1, dva); + Float sign2 = _dot(vec2, dvb); + + if ((sign1 < 0.0) == (sign2 < 0.0)) { + found = true; + + Float len = _dot(vec1, vec1); + + if (len < mindis) { + mindis = len; + mins = s; + minp = evaluate(s); + } + continue; + } + + found = true; + + Float start = s - ds; + Float end = s; + Float mid = (start + end) * 0.5; + const int binary_steps = 10; + + for (int j = 0; j < binary_steps; j++) { + Vector dvmid = derivative(mid, false); + Vector vecmid = evaluate(mid) - p; + Float sign_mid = _dot(vecmid, dvmid); + + if ((sign_mid < 0.0) == (sign1 < 0.0)) { + start = mid; + } + else { + end = mid; + } + mid = (start + end) * 0.5; + } + + Vector p2 = evaluate(mid); + Vector vec_mid = p2 - p; + Float len = _dot(vec_mid, vec_mid); + + if (len < mindis) { + mindis = len; + minp = p2; + mins = mid; + } + } + + if (!found) { + mins = 0.0; + minp = evaluate(mins); + Vector vec = minp - p; + mindis = _dot(vec, vec); + } + + r_tan = derivative(mins, true); + r_s = mins; + r_dis = sqrtf(mindis); + + return minp; + } + + void pop_front(int n = 1) + { + for (int i = 0; i < segments.size() - n; i++) { + segments[i] = segments[i + n]; + } + + segments.resize(segments.size() - n); + update(); + } + + private: + bool need_update; + + Float _dot(Vector a, Vector b) + { + Float sum = 0.0; + + for (int i = 0; i < axes; i++) { + sum += a[i] * b[i]; + } + + return sum; + } + + Float clamp_s(Float s) + { + s = s < 0.0 ? 0.0 : s; + s = s >= length ? length * 0.999999 : s; + + return s; + } + + Segment *get_segment(Float s) + { + // printf("\n"); + + for (Segment &seg : segments) { + // printf("s: %f %f\n", seg.start, seg.start + seg.bezier.length); + + if (s >= seg.start && s < seg.start + seg.bezier.length) { + return &seg; + } + } + + // printf("\n"); + + return nullptr; + } +}; + +using BezierSpline2f = BezierSpline<float, 2>; +using BezierSpline3f = BezierSpline<float, 3>; +} // namespace blender |