diff options
Diffstat (limited to 'scene/resources/curve.cpp')
| -rw-r--r-- | scene/resources/curve.cpp | 958 |
1 files changed, 593 insertions, 365 deletions
diff --git a/scene/resources/curve.cpp b/scene/resources/curve.cpp index c99f71b13e..bc2149a8c6 100644 --- a/scene/resources/curve.cpp +++ b/scene/resources/curve.cpp @@ -31,18 +31,7 @@ #include "curve.h" #include "core/core_string_names.h" - -template <class T> -static _FORCE_INLINE_ T _bezier_interp(real_t p_t, T p_start, T p_control_1, T p_control_2, T p_end) { - /* Formula from Wikipedia article on Bezier curves. */ - real_t omt = (1.0 - p_t); - real_t omt2 = omt * omt; - real_t omt3 = omt2 * omt; - real_t t2 = p_t * p_t; - real_t t3 = t2 * p_t; - - return p_start * omt3 + p_control_1 * omt2 * p_t * 3.0 + p_control_2 * omt * t2 * 3.0 + p_end * t3; -} +#include "core/math/math_funcs.h" const char *Curve::SIGNAL_RANGE_CHANGED = "range_changed"; @@ -56,12 +45,13 @@ void Curve::set_point_count(int p_count) { mark_dirty(); } else { for (int i = p_count - _points.size(); i > 0; i--) { - add_point(Vector2()); + _add_point(Vector2()); } } + notify_property_list_changed(); } -int Curve::add_point(Vector2 p_position, real_t p_left_tangent, real_t p_right_tangent, TangentMode p_left_mode, TangentMode p_right_mode) { +int Curve::_add_point(Vector2 p_position, real_t p_left_tangent, real_t p_right_tangent, TangentMode p_left_mode, TangentMode p_right_mode) { // Add a point and preserve order // Curve bounds is in 0..1 @@ -112,6 +102,13 @@ int Curve::add_point(Vector2 p_position, real_t p_left_tangent, real_t p_right_t return ret; } +int Curve::add_point(Vector2 p_position, real_t p_left_tangent, real_t p_right_tangent, TangentMode p_left_mode, TangentMode p_right_mode) { + int ret = _add_point(p_position, p_left_tangent, p_right_tangent, p_left_mode, p_right_mode); + notify_property_list_changed(); + + return ret; +} + int Curve::get_index(real_t p_offset) const { // Lower-bound float binary search @@ -217,15 +214,21 @@ Curve::TangentMode Curve::get_point_right_mode(int p_index) const { return _points[p_index].right_mode; } -void Curve::remove_point(int p_index) { +void Curve::_remove_point(int p_index) { ERR_FAIL_INDEX(p_index, _points.size()); _points.remove_at(p_index); mark_dirty(); } +void Curve::remove_point(int p_index) { + _remove_point(p_index); + notify_property_list_changed(); +} + void Curve::clear_points() { _points.clear(); mark_dirty(); + notify_property_list_changed(); } void Curve::set_point_value(int p_index, real_t p_position) { @@ -238,8 +241,8 @@ void Curve::set_point_value(int p_index, real_t p_position) { int Curve::set_point_offset(int p_index, real_t p_offset) { ERR_FAIL_INDEX_V(p_index, _points.size(), -1); Point p = _points[p_index]; - remove_point(p_index); - int i = add_point(Vector2(p_offset, p.position.y)); + _remove_point(p_index); + int i = _add_point(Vector2(p_offset, p.position.y)); _points.write[i].left_tangent = p.left_tangent; _points.write[i].right_tangent = p.right_tangent; _points.write[i].left_mode = p.left_mode; @@ -311,7 +314,7 @@ void Curve::set_max_value(real_t p_max) { emit_signal(SNAME(SIGNAL_RANGE_CHANGED)); } -real_t Curve::interpolate(real_t p_offset) const { +real_t Curve::sample(real_t p_offset) const { if (_points.size() == 0) { return 0; } @@ -331,14 +334,14 @@ real_t Curve::interpolate(real_t p_offset) const { return _points[0].position.y; } - return interpolate_local_nocheck(i, local); + return sample_local_nocheck(i, local); } -real_t Curve::interpolate_local_nocheck(int p_index, real_t p_local_offset) const { +real_t Curve::sample_local_nocheck(int p_index, real_t p_local_offset) const { const Point a = _points[p_index]; const Point b = _points[p_index + 1]; - /* Cubic bezier + /* Cubic bézier * * ac-----bc * / \ @@ -362,7 +365,7 @@ real_t Curve::interpolate_local_nocheck(int p_index, real_t p_local_offset) cons real_t yac = a.position.y + d * a.right_tangent; real_t ybc = b.position.y - d * b.left_tangent; - real_t y = _bezier_interp(p_local_offset, a.position.y, yac, ybc, b.position.y); + real_t y = Math::bezier_interpolate(a.position.y, yac, ybc, b.position.y, p_local_offset); return y; } @@ -370,7 +373,6 @@ real_t Curve::interpolate_local_nocheck(int p_index, real_t p_local_offset) cons void Curve::mark_dirty() { _baked_cache_dirty = true; emit_signal(CoreStringNames::get_singleton()->changed); - notify_property_list_changed(); } Array Curve::get_data() const { @@ -429,6 +431,7 @@ void Curve::set_data(const Array p_input) { } mark_dirty(); + notify_property_list_changed(); } void Curve::bake() { @@ -438,7 +441,7 @@ void Curve::bake() { for (int i = 1; i < _bake_resolution - 1; ++i) { real_t x = i / static_cast<real_t>(_bake_resolution); - real_t y = interpolate(x); + real_t y = sample(x); _baked_cache.write[i] = y; } @@ -457,7 +460,7 @@ void Curve::set_bake_resolution(int p_resolution) { _baked_cache_dirty = true; } -real_t Curve::interpolate_baked(real_t p_offset) const { +real_t Curve::sample_baked(real_t p_offset) const { if (_baked_cache_dirty) { // Last-second bake if not done already const_cast<Curve *>(this)->bake(); @@ -484,7 +487,7 @@ real_t Curve::interpolate_baked(real_t p_offset) const { fi = 0; } - // Interpolate + // Sample if (i + 1 < _baked_cache.size()) { real_t t = fi - i; return Math::lerp(_baked_cache[i], _baked_cache[i + 1], t); @@ -593,8 +596,8 @@ void Curve::_bind_methods() { ClassDB::bind_method(D_METHOD("get_point_position", "index"), &Curve::get_point_position); ClassDB::bind_method(D_METHOD("set_point_value", "index", "y"), &Curve::set_point_value); ClassDB::bind_method(D_METHOD("set_point_offset", "index", "offset"), &Curve::set_point_offset); - ClassDB::bind_method(D_METHOD("interpolate", "offset"), &Curve::interpolate); - ClassDB::bind_method(D_METHOD("interpolate_baked", "offset"), &Curve::interpolate_baked); + ClassDB::bind_method(D_METHOD("sample", "offset"), &Curve::sample); + ClassDB::bind_method(D_METHOD("sample_baked", "offset"), &Curve::sample_baked); ClassDB::bind_method(D_METHOD("get_point_left_tangent", "index"), &Curve::get_point_left_tangent); ClassDB::bind_method(D_METHOD("get_point_right_tangent", "index"), &Curve::get_point_right_tangent); ClassDB::bind_method(D_METHOD("get_point_left_mode", "index"), &Curve::get_point_left_mode); @@ -636,16 +639,15 @@ void Curve2D::set_point_count(int p_count) { if (points.size() >= p_count) { points.resize(p_count); mark_dirty(); - baked_cache_dirty = true; - emit_signal(CoreStringNames::get_singleton()->changed); } else { for (int i = p_count - points.size(); i > 0; i--) { - add_point(Vector2()); + _add_point(Vector2()); } } + notify_property_list_changed(); } -void Curve2D::add_point(const Vector2 &p_position, const Vector2 &p_in, const Vector2 &p_out, int p_atpos) { +void Curve2D::_add_point(const Vector2 &p_position, const Vector2 &p_in, const Vector2 &p_out, int p_atpos) { Point n; n.position = p_position; n.in = p_in; @@ -659,6 +661,11 @@ void Curve2D::add_point(const Vector2 &p_position, const Vector2 &p_in, const Ve mark_dirty(); } +void Curve2D::add_point(const Vector2 &p_position, const Vector2 &p_in, const Vector2 &p_out, int p_atpos) { + _add_point(p_position, p_in, p_out, p_atpos); + notify_property_list_changed(); +} + void Curve2D::set_point_position(int p_index, const Vector2 &p_position) { ERR_FAIL_INDEX(p_index, points.size()); @@ -695,20 +702,26 @@ Vector2 Curve2D::get_point_out(int p_index) const { return points[p_index].out; } -void Curve2D::remove_point(int p_index) { +void Curve2D::_remove_point(int p_index) { ERR_FAIL_INDEX(p_index, points.size()); points.remove_at(p_index); mark_dirty(); } +void Curve2D::remove_point(int p_index) { + _remove_point(p_index); + notify_property_list_changed(); +} + void Curve2D::clear_points() { if (!points.is_empty()) { points.clear(); mark_dirty(); + notify_property_list_changed(); } } -Vector2 Curve2D::interpolate(int p_index, const real_t p_offset) const { +Vector2 Curve2D::sample(int p_index, const real_t p_offset) const { int pc = points.size(); ERR_FAIL_COND_V(pc == 0, Vector2()); @@ -723,36 +736,35 @@ Vector2 Curve2D::interpolate(int p_index, const real_t p_offset) const { Vector2 p3 = points[p_index + 1].position; Vector2 p2 = p3 + points[p_index + 1].in; - return _bezier_interp(p_offset, p0, p1, p2, p3); + return p0.bezier_interpolate(p1, p2, p3, p_offset); } -Vector2 Curve2D::interpolatef(real_t p_findex) const { +Vector2 Curve2D::samplef(real_t p_findex) const { if (p_findex < 0) { p_findex = 0; } else if (p_findex >= points.size()) { p_findex = points.size(); } - return interpolate((int)p_findex, Math::fmod(p_findex, (real_t)1.0)); + return sample((int)p_findex, Math::fmod(p_findex, (real_t)1.0)); } void Curve2D::mark_dirty() { baked_cache_dirty = true; emit_signal(CoreStringNames::get_singleton()->changed); - notify_property_list_changed(); } void Curve2D::_bake_segment2d(RBMap<real_t, Vector2> &r_bake, real_t p_begin, real_t p_end, const Vector2 &p_a, const Vector2 &p_out, const Vector2 &p_b, const Vector2 &p_in, int p_depth, int p_max_depth, real_t p_tol) const { real_t mp = p_begin + (p_end - p_begin) * 0.5; - Vector2 beg = _bezier_interp(p_begin, p_a, p_a + p_out, p_b + p_in, p_b); - Vector2 mid = _bezier_interp(mp, p_a, p_a + p_out, p_b + p_in, p_b); - Vector2 end = _bezier_interp(p_end, p_a, p_a + p_out, p_b + p_in, p_b); + Vector2 beg = p_a.bezier_interpolate(p_a + p_out, p_b + p_in, p_b, p_begin); + Vector2 mid = p_a.bezier_interpolate(p_a + p_out, p_b + p_in, p_b, mp); + Vector2 end = p_a.bezier_interpolate(p_a + p_out, p_b + p_in, p_b, p_end); Vector2 na = (mid - beg).normalized(); Vector2 nb = (end - mid).normalized(); real_t dp = na.dot(nb); - if (dp < Math::cos(Math::deg2rad(p_tol))) { + if (dp < Math::cos(Math::deg_to_rad(p_tol))) { r_bake[mp] = mid; } @@ -762,6 +774,22 @@ void Curve2D::_bake_segment2d(RBMap<real_t, Vector2> &r_bake, real_t p_begin, re } } +void Curve2D::_bake_segment2d_even_length(RBMap<real_t, Vector2> &r_bake, real_t p_begin, real_t p_end, const Vector2 &p_a, const Vector2 &p_out, const Vector2 &p_b, const Vector2 &p_in, int p_depth, int p_max_depth, real_t p_length) const { + Vector2 beg = p_a.bezier_interpolate(p_a + p_out, p_b + p_in, p_b, p_begin); + Vector2 end = p_a.bezier_interpolate(p_a + p_out, p_b + p_in, p_b, p_end); + + real_t length = beg.distance_to(end); + + if (length > p_length && p_depth < p_max_depth) { + real_t mp = (p_begin + p_end) * 0.5; + Vector2 mid = p_a.bezier_interpolate(p_a + p_out, p_b + p_in, p_b, mp); + r_bake[mp] = mid; + + _bake_segment2d_even_length(r_bake, p_begin, mp, p_a, p_out, p_b, p_in, p_depth + 1, p_max_depth, p_length); + _bake_segment2d_even_length(r_bake, mp, p_end, p_a, p_out, p_b, p_in, p_depth + 1, p_max_depth, p_length); + } +} + void Curve2D::_bake() const { if (!baked_cache_dirty) { return; @@ -773,94 +801,62 @@ void Curve2D::_bake() const { if (points.size() == 0) { baked_point_cache.clear(); baked_dist_cache.clear(); + baked_forward_vector_cache.clear(); return; } if (points.size() == 1) { baked_point_cache.resize(1); baked_point_cache.set(0, points[0].position); - baked_dist_cache.resize(1); baked_dist_cache.set(0, 0.0); + baked_forward_vector_cache.resize(1); + baked_forward_vector_cache.set(0, Vector2(0.0, 0.1)); + return; } - Vector2 position = points[0].position; - real_t dist = 0.0; - - List<Vector2> pointlist; - List<real_t> distlist; - - // Start always from origin. - pointlist.push_back(position); - distlist.push_back(0.0); - - for (int i = 0; i < points.size() - 1; i++) { - real_t step = 0.1; // at least 10 substeps ought to be enough? - real_t p = 0.0; - - while (p < 1.0) { - real_t np = p + step; - if (np > 1.0) { - np = 1.0; - } + // Tesselate curve to (almost) even length segments + { + Vector<RBMap<real_t, Vector2>> midpoints = _tessellate_even_length(10, bake_interval); - Vector2 npp = _bezier_interp(np, points[i].position, points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position); - real_t d = position.distance_to(npp); - - if (d > bake_interval) { - // OK! between P and NP there _has_ to be Something, let's go searching! - - int iterations = 10; //lots of detail! - - real_t low = p; - real_t hi = np; - real_t mid = low + (hi - low) * 0.5; - - for (int j = 0; j < iterations; j++) { - npp = _bezier_interp(mid, points[i].position, points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position); - d = position.distance_to(npp); - - if (bake_interval < d) { - hi = mid; - } else { - low = mid; - } - mid = low + (hi - low) * 0.5; - } - - position = npp; - p = mid; - dist += d; - - pointlist.push_back(position); - distlist.push_back(dist); - } else { - p = np; - } + int pc = 1; + for (int i = 0; i < points.size() - 1; i++) { + pc++; + pc += midpoints[i].size(); } - Vector2 npp = points[i + 1].position; - real_t d = position.distance_to(npp); - - position = npp; - dist += d; + baked_point_cache.resize(pc); + baked_dist_cache.resize(pc); + baked_forward_vector_cache.resize(pc); - pointlist.push_back(position); - distlist.push_back(dist); - } + Vector2 *bpw = baked_point_cache.ptrw(); + Vector2 *bfw = baked_forward_vector_cache.ptrw(); - baked_max_ofs = dist; + // Collect positions and sample tilts and tangents for each baked points. + bpw[0] = points[0].position; + bfw[0] = points[0].position.bezier_derivative(points[0].position + points[0].out, points[1].position + points[1].in, points[1].position, 0.0).normalized(); + int pidx = 0; - baked_point_cache.resize(pointlist.size()); - baked_dist_cache.resize(distlist.size()); + for (int i = 0; i < points.size() - 1; i++) { + for (const KeyValue<real_t, Vector2> &E : midpoints[i]) { + pidx++; + bpw[pidx] = E.value; + bfw[pidx] = points[i].position.bezier_derivative(points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position, E.key).normalized(); + } - Vector2 *w = baked_point_cache.ptrw(); - real_t *wd = baked_dist_cache.ptrw(); + pidx++; + bpw[pidx] = points[i + 1].position; + bfw[pidx] = points[i].position.bezier_derivative(points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position, 1.0).normalized(); + } - for (int i = 0; i < pointlist.size(); i++) { - w[i] = pointlist[i]; - wd[i] = distlist[i]; + // Recalculate the baked distances. + real_t *bdw = baked_dist_cache.ptrw(); + bdw[0] = 0.0; + for (int i = 0; i < pc - 1; i++) { + bdw[i + 1] = bdw[i] + bpw[i].distance_to(bpw[i + 1]); + } + baked_max_ofs = bdw[pc - 1]; } } @@ -872,27 +868,15 @@ real_t Curve2D::get_baked_length() const { return baked_max_ofs; } -Vector2 Curve2D::interpolate_baked(real_t p_offset, bool p_cubic) const { - if (baked_cache_dirty) { - _bake(); - } +Curve2D::Interval Curve2D::_find_interval(real_t p_offset) const { + Interval interval = { + -1, + 0.0 + }; + ERR_FAIL_COND_V_MSG(baked_cache_dirty, interval, "Backed cache is dirty"); - // Validate: Curve may not have baked points. int pc = baked_point_cache.size(); - ERR_FAIL_COND_V_MSG(pc == 0, Vector2(), "No points in Curve2D."); - - if (pc == 1) { - return baked_point_cache.get(0); - } - - const Vector2 *r = baked_point_cache.ptr(); - - if (p_offset < 0) { - return r[0]; - } - if (p_offset >= baked_max_ofs) { - return r[pc - 1]; - } + ERR_FAIL_COND_V_MSG(pc < 2, interval, "Less than two points in cache"); int start = 0; int end = pc; @@ -912,9 +896,27 @@ Vector2 Curve2D::interpolate_baked(real_t p_offset, bool p_cubic) const { real_t offset_end = baked_dist_cache[idx + 1]; real_t idx_interval = offset_end - offset_begin; - ERR_FAIL_COND_V_MSG(p_offset < offset_begin || p_offset > offset_end, Vector2(), "failed to find baked segment"); + ERR_FAIL_COND_V_MSG(p_offset < offset_begin || p_offset > offset_end, interval, "Offset out of range."); + + interval.idx = idx; + if (idx_interval < FLT_EPSILON) { + interval.frac = 0.5; // For a very short interval, 0.5 is a reasonable choice. + ERR_FAIL_V_MSG(interval, "Zero length interval."); + } + + interval.frac = (p_offset - offset_begin) / idx_interval; + return interval; +} - real_t frac = (p_offset - offset_begin) / idx_interval; +Vector2 Curve2D::_sample_baked(Interval p_interval, bool p_cubic) const { + // Assuming p_interval is valid. + ERR_FAIL_INDEX_V_MSG(p_interval.idx, baked_point_cache.size(), Vector2(), "Invalid interval"); + + int idx = p_interval.idx; + real_t frac = p_interval.frac; + + const Vector2 *r = baked_point_cache.ptr(); + int pc = baked_point_cache.size(); if (p_cubic) { Vector2 pre = idx > 0 ? r[idx - 1] : r[idx]; @@ -925,6 +927,72 @@ Vector2 Curve2D::interpolate_baked(real_t p_offset, bool p_cubic) const { } } +Transform2D Curve2D::_sample_posture(Interval p_interval) const { + // Assuming that p_interval is valid. + ERR_FAIL_INDEX_V_MSG(p_interval.idx, baked_point_cache.size(), Transform2D(), "Invalid interval"); + + int idx = p_interval.idx; + real_t frac = p_interval.frac; + + Vector2 forward_begin = baked_forward_vector_cache[idx]; + Vector2 forward_end = baked_forward_vector_cache[idx + 1]; + + // Build frames at both ends of the interval, then interpolate. + const Vector2 forward = forward_begin.slerp(forward_end, frac).normalized(); + const Vector2 side = Vector2(-forward.y, forward.x); + + return Transform2D(side, forward, Vector2(0.0, 0.0)); +} + +Vector2 Curve2D::sample_baked(real_t p_offset, bool p_cubic) const { + if (baked_cache_dirty) { + _bake(); + } + + // Validate: Curve may not have baked points. + int pc = baked_point_cache.size(); + ERR_FAIL_COND_V_MSG(pc == 0, Vector2(), "No points in Curve2D."); + + if (pc == 1) { + return baked_point_cache[0]; + } + + p_offset = CLAMP(p_offset, 0.0, get_baked_length()); // PathFollower implement wrapping logic. + + Curve2D::Interval interval = _find_interval(p_offset); + return _sample_baked(interval, p_cubic); +} + +Transform2D Curve2D::sample_baked_with_rotation(real_t p_offset, bool p_cubic) const { + if (baked_cache_dirty) { + _bake(); + } + + // Validate: Curve may not have baked points. + const int point_count = baked_point_cache.size(); + ERR_FAIL_COND_V_MSG(point_count == 0, Transform2D(), "No points in Curve3D."); + + if (point_count == 1) { + Transform2D t; + t.set_origin(baked_point_cache.get(0)); + ERR_FAIL_V_MSG(t, "Only 1 point in Curve2D."); + } + + p_offset = CLAMP(p_offset, 0.0, get_baked_length()); // PathFollower implement wrapping logic. + + // 0. Find interval for all sampling steps. + Curve2D::Interval interval = _find_interval(p_offset); + + // 1. Sample position. + Vector2 pos = _sample_baked(interval, p_cubic); + + // 2. Sample rotation frame. + Transform2D frame = _sample_posture(interval); + frame.set_origin(pos); + + return frame; +} + PackedVector2Array Curve2D::get_baked_points() const { if (baked_cache_dirty) { _bake(); @@ -1054,7 +1122,8 @@ void Curve2D::_set_data(const Dictionary &p_data) { points.write[i].position = r[i * 3 + 2]; } - baked_cache_dirty = true; + mark_dirty(); + notify_property_list_changed(); } PackedVector2Array Curve2D::tessellate(int p_max_stages, real_t p_tolerance) const { @@ -1094,6 +1163,50 @@ PackedVector2Array Curve2D::tessellate(int p_max_stages, real_t p_tolerance) con return tess; } +Vector<RBMap<real_t, Vector2>> Curve2D::_tessellate_even_length(int p_max_stages, real_t p_length) const { + Vector<RBMap<real_t, Vector2>> midpoints; + ERR_FAIL_COND_V_MSG(points.size() < 2, midpoints, "Curve must have at least 2 control point"); + + midpoints.resize(points.size() - 1); + + for (int i = 0; i < points.size() - 1; i++) { + _bake_segment2d_even_length(midpoints.write[i], 0, 1, points[i].position, points[i].out, points[i + 1].position, points[i + 1].in, 0, p_max_stages, p_length); + } + return midpoints; +} + +PackedVector2Array Curve2D::tessellate_even_length(int p_max_stages, real_t p_length) const { + PackedVector2Array tess; + + Vector<RBMap<real_t, Vector2>> midpoints = _tessellate_even_length(p_max_stages, p_length); + if (midpoints.size() == 0) { + return tess; + } + + int pc = 1; + for (int i = 0; i < points.size() - 1; i++) { + pc++; + pc += midpoints[i].size(); + } + + tess.resize(pc); + Vector2 *bpw = tess.ptrw(); + bpw[0] = points[0].position; + int pidx = 0; + + for (int i = 0; i < points.size() - 1; i++) { + for (const KeyValue<real_t, Vector2> &E : midpoints[i]) { + pidx++; + bpw[pidx] = E.value; + } + + pidx++; + bpw[pidx] = points[i + 1].position; + } + + return tess; +} + bool Curve2D::_set(const StringName &p_name, const Variant &p_value) { Vector<String> components = String(p_name).split("/", true, 2); if (components.size() >= 2 && components[0].begins_with("point_") && components[0].trim_prefix("point_").is_valid_int()) { @@ -1155,7 +1268,7 @@ void Curve2D::_get_property_list(List<PropertyInfo> *p_list) const { void Curve2D::_bind_methods() { ClassDB::bind_method(D_METHOD("get_point_count"), &Curve2D::get_point_count); ClassDB::bind_method(D_METHOD("set_point_count", "count"), &Curve2D::set_point_count); - ClassDB::bind_method(D_METHOD("add_point", "position", "in", "out", "at_position"), &Curve2D::add_point, DEFVAL(Vector2()), DEFVAL(Vector2()), DEFVAL(-1)); + ClassDB::bind_method(D_METHOD("add_point", "position", "in", "out", "index"), &Curve2D::add_point, DEFVAL(Vector2()), DEFVAL(Vector2()), DEFVAL(-1)); ClassDB::bind_method(D_METHOD("set_point_position", "idx", "position"), &Curve2D::set_point_position); ClassDB::bind_method(D_METHOD("get_point_position", "idx"), &Curve2D::get_point_position); ClassDB::bind_method(D_METHOD("set_point_in", "idx", "position"), &Curve2D::set_point_in); @@ -1164,21 +1277,23 @@ void Curve2D::_bind_methods() { ClassDB::bind_method(D_METHOD("get_point_out", "idx"), &Curve2D::get_point_out); ClassDB::bind_method(D_METHOD("remove_point", "idx"), &Curve2D::remove_point); ClassDB::bind_method(D_METHOD("clear_points"), &Curve2D::clear_points); - ClassDB::bind_method(D_METHOD("interpolate", "idx", "t"), &Curve2D::interpolate); - ClassDB::bind_method(D_METHOD("interpolatef", "fofs"), &Curve2D::interpolatef); + ClassDB::bind_method(D_METHOD("sample", "idx", "t"), &Curve2D::sample); + ClassDB::bind_method(D_METHOD("samplef", "fofs"), &Curve2D::samplef); //ClassDB::bind_method(D_METHOD("bake","subdivs"),&Curve2D::bake,DEFVAL(10)); ClassDB::bind_method(D_METHOD("set_bake_interval", "distance"), &Curve2D::set_bake_interval); ClassDB::bind_method(D_METHOD("get_bake_interval"), &Curve2D::get_bake_interval); ClassDB::bind_method(D_METHOD("get_baked_length"), &Curve2D::get_baked_length); - ClassDB::bind_method(D_METHOD("interpolate_baked", "offset", "cubic"), &Curve2D::interpolate_baked, DEFVAL(false)); + ClassDB::bind_method(D_METHOD("sample_baked", "offset", "cubic"), &Curve2D::sample_baked, DEFVAL(0.0), DEFVAL(false)); + ClassDB::bind_method(D_METHOD("sample_baked_with_rotation", "offset", "cubic"), &Curve2D::sample_baked_with_rotation, DEFVAL(0.0), DEFVAL(false)); ClassDB::bind_method(D_METHOD("get_baked_points"), &Curve2D::get_baked_points); ClassDB::bind_method(D_METHOD("get_closest_point", "to_point"), &Curve2D::get_closest_point); ClassDB::bind_method(D_METHOD("get_closest_offset", "to_point"), &Curve2D::get_closest_offset); ClassDB::bind_method(D_METHOD("tessellate", "max_stages", "tolerance_degrees"), &Curve2D::tessellate, DEFVAL(5), DEFVAL(4)); + ClassDB::bind_method(D_METHOD("tessellate_even_length", "max_stages", "tolerance_length"), &Curve2D::tessellate_even_length, DEFVAL(5), DEFVAL(20.0)); ClassDB::bind_method(D_METHOD("_get_data"), &Curve2D::_get_data); - ClassDB::bind_method(D_METHOD("_set_data"), &Curve2D::_set_data); + ClassDB::bind_method(D_METHOD("_set_data", "data"), &Curve2D::_set_data); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "bake_interval", PROPERTY_HINT_RANGE, "0.01,512,0.01"), "set_bake_interval", "get_bake_interval"); ADD_PROPERTY(PropertyInfo(Variant::INT, "_data", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR | PROPERTY_USAGE_INTERNAL), "_set_data", "_get_data"); @@ -1205,12 +1320,13 @@ void Curve3D::set_point_count(int p_count) { mark_dirty(); } else { for (int i = p_count - points.size(); i > 0; i--) { - add_point(Vector3()); + _add_point(Vector3()); } } + notify_property_list_changed(); } -void Curve3D::add_point(const Vector3 &p_position, const Vector3 &p_in, const Vector3 &p_out, int p_atpos) { +void Curve3D::_add_point(const Vector3 &p_position, const Vector3 &p_in, const Vector3 &p_out, int p_atpos) { Point n; n.position = p_position; n.in = p_in; @@ -1224,6 +1340,11 @@ void Curve3D::add_point(const Vector3 &p_position, const Vector3 &p_in, const Ve mark_dirty(); } +void Curve3D::add_point(const Vector3 &p_position, const Vector3 &p_in, const Vector3 &p_out, int p_atpos) { + _add_point(p_position, p_in, p_out, p_atpos); + notify_property_list_changed(); +} + void Curve3D::set_point_position(int p_index, const Vector3 &p_position) { ERR_FAIL_INDEX(p_index, points.size()); @@ -1272,20 +1393,26 @@ Vector3 Curve3D::get_point_out(int p_index) const { return points[p_index].out; } -void Curve3D::remove_point(int p_index) { +void Curve3D::_remove_point(int p_index) { ERR_FAIL_INDEX(p_index, points.size()); points.remove_at(p_index); mark_dirty(); } +void Curve3D::remove_point(int p_index) { + _remove_point(p_index); + notify_property_list_changed(); +} + void Curve3D::clear_points() { if (!points.is_empty()) { points.clear(); mark_dirty(); + notify_property_list_changed(); } } -Vector3 Curve3D::interpolate(int p_index, real_t p_offset) const { +Vector3 Curve3D::sample(int p_index, real_t p_offset) const { int pc = points.size(); ERR_FAIL_COND_V(pc == 0, Vector3()); @@ -1300,36 +1427,35 @@ Vector3 Curve3D::interpolate(int p_index, real_t p_offset) const { Vector3 p3 = points[p_index + 1].position; Vector3 p2 = p3 + points[p_index + 1].in; - return _bezier_interp(p_offset, p0, p1, p2, p3); + return p0.bezier_interpolate(p1, p2, p3, p_offset); } -Vector3 Curve3D::interpolatef(real_t p_findex) const { +Vector3 Curve3D::samplef(real_t p_findex) const { if (p_findex < 0) { p_findex = 0; } else if (p_findex >= points.size()) { p_findex = points.size(); } - return interpolate((int)p_findex, Math::fmod(p_findex, (real_t)1.0)); + return sample((int)p_findex, Math::fmod(p_findex, (real_t)1.0)); } void Curve3D::mark_dirty() { baked_cache_dirty = true; emit_signal(CoreStringNames::get_singleton()->changed); - notify_property_list_changed(); } void Curve3D::_bake_segment3d(RBMap<real_t, Vector3> &r_bake, real_t p_begin, real_t p_end, const Vector3 &p_a, const Vector3 &p_out, const Vector3 &p_b, const Vector3 &p_in, int p_depth, int p_max_depth, real_t p_tol) const { real_t mp = p_begin + (p_end - p_begin) * 0.5; - Vector3 beg = _bezier_interp(p_begin, p_a, p_a + p_out, p_b + p_in, p_b); - Vector3 mid = _bezier_interp(mp, p_a, p_a + p_out, p_b + p_in, p_b); - Vector3 end = _bezier_interp(p_end, p_a, p_a + p_out, p_b + p_in, p_b); + Vector3 beg = p_a.bezier_interpolate(p_a + p_out, p_b + p_in, p_b, p_begin); + Vector3 mid = p_a.bezier_interpolate(p_a + p_out, p_b + p_in, p_b, mp); + Vector3 end = p_a.bezier_interpolate(p_a + p_out, p_b + p_in, p_b, p_end); Vector3 na = (mid - beg).normalized(); Vector3 nb = (end - mid).normalized(); real_t dp = na.dot(nb); - if (dp < Math::cos(Math::deg2rad(p_tol))) { + if (dp < Math::cos(Math::deg_to_rad(p_tol))) { r_bake[mp] = mid; } if (p_depth < p_max_depth) { @@ -1338,6 +1464,22 @@ void Curve3D::_bake_segment3d(RBMap<real_t, Vector3> &r_bake, real_t p_begin, re } } +void Curve3D::_bake_segment3d_even_length(RBMap<real_t, Vector3> &r_bake, real_t p_begin, real_t p_end, const Vector3 &p_a, const Vector3 &p_out, const Vector3 &p_b, const Vector3 &p_in, int p_depth, int p_max_depth, real_t p_length) const { + Vector3 beg = p_a.bezier_interpolate(p_a + p_out, p_b + p_in, p_b, p_begin); + Vector3 end = p_a.bezier_interpolate(p_a + p_out, p_b + p_in, p_b, p_end); + + real_t length = beg.distance_to(end); + + if (length > p_length && p_depth < p_max_depth) { + real_t mp = (p_begin + p_end) * 0.5; + Vector3 mid = p_a.bezier_interpolate(p_a + p_out, p_b + p_in, p_b, mp); + r_bake[mp] = mid; + + _bake_segment3d_even_length(r_bake, p_begin, mp, p_a, p_out, p_b, p_in, p_depth + 1, p_max_depth, p_length); + _bake_segment3d_even_length(r_bake, mp, p_end, p_a, p_out, p_b, p_in, p_depth + 1, p_max_depth, p_length); + } +} + void Curve3D::_bake() const { if (!baked_cache_dirty) { return; @@ -1349,8 +1491,10 @@ void Curve3D::_bake() const { if (points.size() == 0) { baked_point_cache.clear(); baked_tilt_cache.clear(); - baked_up_vector_cache.clear(); baked_dist_cache.clear(); + + baked_forward_vector_cache.clear(); + baked_up_vector_cache.clear(); return; } @@ -1361,10 +1505,12 @@ void Curve3D::_bake() const { baked_tilt_cache.set(0, points[0].tilt); baked_dist_cache.resize(1); baked_dist_cache.set(0, 0.0); + baked_forward_vector_cache.resize(1); + baked_forward_vector_cache.set(0, Vector3(0.0, 0.0, 1.0)); if (up_vector_enabled) { baked_up_vector_cache.resize(1); - baked_up_vector_cache.set(0, Vector3(0, 1, 0)); + baked_up_vector_cache.set(0, Vector3(0.0, 1.0, 0.0)); } else { baked_up_vector_cache.clear(); } @@ -1372,136 +1518,135 @@ void Curve3D::_bake() const { return; } - Vector3 position = points[0].position; - real_t dist = 0.0; - List<Plane> pointlist; - List<real_t> distlist; + // Step 1: Tesselate curve to (almost) even length segments + { + Vector<RBMap<real_t, Vector3>> midpoints = _tessellate_even_length(10, bake_interval); - // Start always from origin. - pointlist.push_back(Plane(position, points[0].tilt)); - distlist.push_back(0.0); - - for (int i = 0; i < points.size() - 1; i++) { - real_t step = 0.1; // at least 10 substeps ought to be enough? - real_t p = 0.0; + int pc = 1; + for (int i = 0; i < points.size() - 1; i++) { + pc++; + pc += midpoints[i].size(); + } - while (p < 1.0) { - real_t np = p + step; - if (np > 1.0) { - np = 1.0; + baked_point_cache.resize(pc); + baked_tilt_cache.resize(pc); + baked_dist_cache.resize(pc); + baked_forward_vector_cache.resize(pc); + + Vector3 *bpw = baked_point_cache.ptrw(); + real_t *btw = baked_tilt_cache.ptrw(); + Vector3 *bfw = baked_forward_vector_cache.ptrw(); + + // Collect positions and sample tilts and tangents for each baked points. + bpw[0] = points[0].position; + bfw[0] = points[0].position.bezier_derivative(points[0].position + points[0].out, points[1].position + points[1].in, points[1].position, 0.0).normalized(); + btw[0] = points[0].tilt; + int pidx = 0; + + for (int i = 0; i < points.size() - 1; i++) { + for (const KeyValue<real_t, Vector3> &E : midpoints[i]) { + pidx++; + bpw[pidx] = E.value; + bfw[pidx] = points[i].position.bezier_derivative(points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position, E.key).normalized(); + btw[pidx] = Math::lerp(points[i].tilt, points[i + 1].tilt, E.key); } - Vector3 npp = _bezier_interp(np, points[i].position, points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position); - real_t d = position.distance_to(npp); - - if (d > bake_interval) { - // OK! between P and NP there _has_ to be Something, let's go searching! - - int iterations = 10; //lots of detail! - - real_t low = p; - real_t hi = np; - real_t mid = low + (hi - low) * 0.5; - - for (int j = 0; j < iterations; j++) { - npp = _bezier_interp(mid, points[i].position, points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position); - d = position.distance_to(npp); - - if (bake_interval < d) { - hi = mid; - } else { - low = mid; - } - mid = low + (hi - low) * 0.5; - } - - position = npp; - p = mid; - Plane post; - post.normal = position; - post.d = Math::lerp(points[i].tilt, points[i + 1].tilt, mid); - dist += d; - - pointlist.push_back(post); - distlist.push_back(dist); - } else { - p = np; - } + pidx++; + bpw[pidx] = points[i + 1].position; + bfw[pidx] = points[i].position.bezier_derivative(points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position, 1.0).normalized(); + btw[pidx] = points[i + 1].tilt; } - Vector3 npp = points[i + 1].position; - real_t d = position.distance_to(npp); - - position = npp; - Plane post; - post.normal = position; - post.d = points[i + 1].tilt; - - dist += d; - - pointlist.push_back(post); - distlist.push_back(dist); + // Recalculate the baked distances. + real_t *bdw = baked_dist_cache.ptrw(); + bdw[0] = 0.0; + for (int i = 0; i < pc - 1; i++) { + bdw[i + 1] = bdw[i] + bpw[i].distance_to(bpw[i + 1]); + } + baked_max_ofs = bdw[pc - 1]; } - baked_max_ofs = dist; - - baked_point_cache.resize(pointlist.size()); - Vector3 *w = baked_point_cache.ptrw(); - int idx = 0; + if (!up_vector_enabled) { + baked_up_vector_cache.resize(0); + return; + } - baked_tilt_cache.resize(pointlist.size()); - real_t *wt = baked_tilt_cache.ptrw(); + // Step 2: Calculate the up vectors and the whole local reference frame + // + // See Dougan, Carl. "The parallel transport frame." Game Programming Gems 2 (2001): 215-219. + // for an example discussing about why not the Frenet frame. + { + int point_count = baked_point_cache.size(); - baked_up_vector_cache.resize(up_vector_enabled ? pointlist.size() : 0); - Vector3 *up_write = baked_up_vector_cache.ptrw(); + baked_up_vector_cache.resize(point_count); + Vector3 *up_write = baked_up_vector_cache.ptrw(); - baked_dist_cache.resize(pointlist.size()); - real_t *wd = baked_dist_cache.ptrw(); + const Vector3 *forward_ptr = baked_forward_vector_cache.ptr(); + const Vector3 *points_ptr = baked_point_cache.ptr(); - Vector3 sideways; - Vector3 up; - Vector3 forward; + Basis frame; // X-right, Y-up, Z-forward. + Basis frame_prev; - Vector3 prev_sideways = Vector3(1, 0, 0); - Vector3 prev_up = Vector3(0, 1, 0); - Vector3 prev_forward = Vector3(0, 0, 1); + // Set the initial frame based on Y-up rule. + { + Vector3 forward = forward_ptr[0]; - for (const Plane &E : pointlist) { - w[idx] = E.normal; - wt[idx] = E.d; - wd[idx] = distlist[idx]; + if (abs(forward.dot(Vector3(0, 1, 0))) > 1.0 - UNIT_EPSILON) { + frame_prev = Basis::looking_at(-forward, Vector3(1, 0, 0)); + } else { + frame_prev = Basis::looking_at(-forward, Vector3(0, 1, 0)); + } - if (!up_vector_enabled) { - idx++; - continue; + up_write[0] = frame_prev.get_column(1); } - forward = idx > 0 ? (w[idx] - w[idx - 1]).normalized() : prev_forward; + // Calculate the Parallel Transport Frame. + for (int idx = 1; idx < point_count; idx++) { + Vector3 forward = forward_ptr[idx]; - real_t y_dot = prev_up.dot(forward); + Basis rotate; + rotate.rotate_to_align(frame_prev.get_column(2), forward); + frame = rotate * frame_prev; + frame.orthonormalize(); // guard against float error accumulation - if (y_dot > (1.0f - CMP_EPSILON)) { - sideways = prev_sideways; - up = -prev_forward; - } else if (y_dot < -(1.0f - CMP_EPSILON)) { - sideways = prev_sideways; - up = prev_forward; - } else { - sideways = prev_up.cross(forward).normalized(); - up = forward.cross(sideways).normalized(); + up_write[idx] = frame.get_column(1); + frame_prev = frame; } - if (idx == 1) { - up_write[0] = up; + bool is_loop = true; + // Loop smoothing only applies when the curve is a loop, which means two ends meet, and share forward directions. + { + if (!points_ptr[0].is_equal_approx(points_ptr[point_count - 1])) { + is_loop = false; + } + + real_t dot = forward_ptr[0].dot(forward_ptr[point_count - 1]); + if (dot < 1.0 - UNIT_EPSILON) { // Alignment should not be too tight, or it dosen't work for coarse bake interval. + is_loop = false; + } } - up_write[idx] = up; + // Twist up vectors, so that they align at two ends of the curve. + if (is_loop) { + const Vector3 up_start = up_write[0]; + const Vector3 up_end = up_write[point_count - 1]; + + real_t sign = SIGN(up_end.cross(up_start).dot(forward_ptr[0])); + real_t full_angle = Quaternion(up_end, up_start).get_angle(); - prev_sideways = sideways; - prev_up = up; - prev_forward = forward; + if (abs(full_angle) < CMP_EPSILON) { + return; + } else { + const real_t *dists = baked_dist_cache.ptr(); + for (int idx = 1; idx < point_count; idx++) { + const real_t frac = dists[idx] / baked_max_ofs; + const real_t angle = Math::lerp((real_t)0.0, full_angle, frac); + Basis twist(forward_ptr[idx] * sign, angle); - idx++; + up_write[idx] = twist.xform(up_write[idx]); + } + } + } } } @@ -1513,27 +1658,15 @@ real_t Curve3D::get_baked_length() const { return baked_max_ofs; } -Vector3 Curve3D::interpolate_baked(real_t p_offset, bool p_cubic) const { - if (baked_cache_dirty) { - _bake(); - } +Curve3D::Interval Curve3D::_find_interval(real_t p_offset) const { + Interval interval = { + -1, + 0.0 + }; + ERR_FAIL_COND_V_MSG(baked_cache_dirty, interval, "Backed cache is dirty"); - // Validate: Curve may not have baked points. int pc = baked_point_cache.size(); - ERR_FAIL_COND_V_MSG(pc == 0, Vector3(), "No points in Curve3D."); - - if (pc == 1) { - return baked_point_cache.get(0); - } - - const Vector3 *r = baked_point_cache.ptr(); - - if (p_offset < 0) { - return r[0]; - } - if (p_offset >= baked_max_ofs) { - return r[pc - 1]; - } + ERR_FAIL_COND_V_MSG(pc < 2, interval, "Less than two points in cache"); int start = 0; int end = pc; @@ -1553,9 +1686,27 @@ Vector3 Curve3D::interpolate_baked(real_t p_offset, bool p_cubic) const { real_t offset_end = baked_dist_cache[idx + 1]; real_t idx_interval = offset_end - offset_begin; - ERR_FAIL_COND_V_MSG(p_offset < offset_begin || p_offset > offset_end, Vector3(), "failed to find baked segment"); + ERR_FAIL_COND_V_MSG(p_offset < offset_begin || p_offset > offset_end, interval, "Offset out of range."); - real_t frac = (p_offset - offset_begin) / idx_interval; + interval.idx = idx; + if (idx_interval < FLT_EPSILON) { + interval.frac = 0.5; // For a very short interval, 0.5 is a reasonable choice. + ERR_FAIL_V_MSG(interval, "Zero length interval."); + } + + interval.frac = (p_offset - offset_begin) / idx_interval; + return interval; +} + +Vector3 Curve3D::_sample_baked(Interval p_interval, bool p_cubic) const { + // Assuming p_interval is valid. + ERR_FAIL_INDEX_V_MSG(p_interval.idx, baked_point_cache.size(), Vector3(), "Invalid interval"); + + int idx = p_interval.idx; + real_t frac = p_interval.frac; + + const Vector3 *r = baked_point_cache.ptr(); + int pc = baked_point_cache.size(); if (p_cubic) { Vector3 pre = idx > 0 ? r[idx - 1] : r[idx]; @@ -1566,114 +1717,142 @@ Vector3 Curve3D::interpolate_baked(real_t p_offset, bool p_cubic) const { } } -real_t Curve3D::interpolate_baked_tilt(real_t p_offset) const { - if (baked_cache_dirty) { - _bake(); - } +real_t Curve3D::_sample_baked_tilt(Interval p_interval) const { + // Assuming that p_interval is valid. + ERR_FAIL_INDEX_V_MSG(p_interval.idx, baked_tilt_cache.size(), 0.0, "Invalid interval"); - // Validate: Curve may not have baked tilts. - int pc = baked_tilt_cache.size(); - ERR_FAIL_COND_V_MSG(pc == 0, 0, "No tilts in Curve3D."); + int idx = p_interval.idx; + real_t frac = p_interval.frac; - if (pc == 1) { - return baked_tilt_cache.get(0); + const real_t *r = baked_tilt_cache.ptr(); + + return Math::lerp(r[idx], r[idx + 1], frac); +} + +Basis Curve3D::_sample_posture(Interval p_interval, bool p_apply_tilt) const { + // Assuming that p_interval is valid. + ERR_FAIL_INDEX_V_MSG(p_interval.idx, baked_point_cache.size(), Basis(), "Invalid interval"); + if (up_vector_enabled) { + ERR_FAIL_INDEX_V_MSG(p_interval.idx, baked_up_vector_cache.size(), Basis(), "Invalid interval"); } - const real_t *r = baked_tilt_cache.ptr(); + int idx = p_interval.idx; + real_t frac = p_interval.frac; + + Vector3 forward_begin = baked_forward_vector_cache[idx]; + Vector3 forward_end = baked_forward_vector_cache[idx + 1]; - if (p_offset < 0) { - return r[0]; + Vector3 up_begin; + Vector3 up_end; + if (up_vector_enabled) { + up_begin = baked_up_vector_cache[idx]; + up_end = baked_up_vector_cache[idx + 1]; + } else { + up_begin = Vector3(0.0, 1.0, 0.0); + up_end = Vector3(0.0, 1.0, 0.0); } - if (p_offset >= baked_max_ofs) { - return r[pc - 1]; + + // Build frames at both ends of the interval, then interpolate. + const Basis frame_begin = Basis::looking_at(-forward_begin, up_begin); + const Basis frame_end = Basis::looking_at(-forward_end, up_end); + const Basis frame = frame_begin.slerp(frame_end, frac).orthonormalized(); + + if (!p_apply_tilt) { + return frame; } - int start = 0; - int end = pc; - int idx = (end + start) / 2; - // Binary search to find baked points. - while (start < idx) { - real_t offset = baked_dist_cache[idx]; - if (p_offset <= offset) { - end = idx; - } else { - start = idx; - } - idx = (end + start) / 2; + // Applying tilt. + const real_t tilt = _sample_baked_tilt(p_interval); + Vector3 forward = frame.get_column(2); + + const Basis twist(forward, tilt); + return twist * frame; +} + +Vector3 Curve3D::sample_baked(real_t p_offset, bool p_cubic) const { + if (baked_cache_dirty) { + _bake(); } - real_t offset_begin = baked_dist_cache[idx]; - real_t offset_end = baked_dist_cache[idx + 1]; + // Validate: Curve may not have baked points. + int pc = baked_point_cache.size(); + ERR_FAIL_COND_V_MSG(pc == 0, Vector3(), "No points in Curve3D."); - real_t idx_interval = offset_end - offset_begin; - ERR_FAIL_COND_V_MSG(p_offset < offset_begin || p_offset > offset_end, 0, "failed to find baked segment"); + if (pc == 1) { + return baked_point_cache[0]; + } - real_t frac = (p_offset - offset_begin) / idx_interval; + p_offset = CLAMP(p_offset, 0.0, get_baked_length()); // PathFollower implement wrapping logic. - return Math::lerp(r[idx], r[idx + 1], (real_t)frac); + Curve3D::Interval interval = _find_interval(p_offset); + return _sample_baked(interval, p_cubic); } -Vector3 Curve3D::interpolate_baked_up_vector(real_t p_offset, bool p_apply_tilt) const { +Transform3D Curve3D::sample_baked_with_rotation(real_t p_offset, bool p_cubic, bool p_apply_tilt) const { if (baked_cache_dirty) { _bake(); } - // Validate: Curve may not have baked up vectors. - int count = baked_up_vector_cache.size(); - ERR_FAIL_COND_V_MSG(count == 0, Vector3(0, 1, 0), "No up vectors in Curve3D."); + // Validate: Curve may not have baked points. + const int point_count = baked_point_cache.size(); + ERR_FAIL_COND_V_MSG(point_count == 0, Transform3D(), "No points in Curve3D."); - if (count == 1) { - return baked_up_vector_cache.get(0); + if (point_count == 1) { + Transform3D t; + t.origin = baked_point_cache.get(0); + ERR_FAIL_V_MSG(t, "Only 1 point in Curve3D."); } - const Vector3 *r = baked_up_vector_cache.ptr(); - const Vector3 *rp = baked_point_cache.ptr(); - const real_t *rt = baked_tilt_cache.ptr(); + p_offset = CLAMP(p_offset, 0.0, get_baked_length()); // PathFollower implement wrapping logic. - int start = 0; - int end = count; - int idx = (end + start) / 2; - // Binary search to find baked points. - while (start < idx) { - real_t offset = baked_dist_cache[idx]; - if (p_offset <= offset) { - end = idx; - } else { - start = idx; - } - idx = (end + start) / 2; - } + // 0. Find interval for all sampling steps. + Curve3D::Interval interval = _find_interval(p_offset); - real_t offset_begin = baked_dist_cache[idx]; - real_t offset_end = baked_dist_cache[idx + 1]; + // 1. Sample position. + Vector3 pos = _sample_baked(interval, p_cubic); - real_t idx_interval = offset_end - offset_begin; - ERR_FAIL_COND_V_MSG(p_offset < offset_begin || p_offset > offset_end, Vector3(0, 1, 0), "failed to find baked segment"); + // 2. Sample rotation frame. + Basis frame = _sample_posture(interval, p_apply_tilt); - real_t frac = (p_offset - offset_begin) / idx_interval; + return Transform3D(frame, pos); +} - if (idx == count - 1) { - return p_apply_tilt ? r[idx].rotated((rp[idx] - rp[idx - 1]).normalized(), rt[idx]) : r[idx]; +real_t Curve3D::sample_baked_tilt(real_t p_offset) const { + if (baked_cache_dirty) { + _bake(); } - Vector3 forward = (rp[idx + 1] - rp[idx]).normalized(); - Vector3 up = r[idx]; - Vector3 up1 = r[idx + 1]; + // Validate: Curve may not have baked tilts. + int pc = baked_tilt_cache.size(); + ERR_FAIL_COND_V_MSG(pc == 0, 0, "No tilts in Curve3D."); - if (p_apply_tilt) { - up.rotate(forward, rt[idx]); - up1.rotate(idx + 2 >= count ? forward : (rp[idx + 2] - rp[idx + 1]).normalized(), rt[idx + 1]); + if (pc == 1) { + return baked_tilt_cache.get(0); } - Vector3 axis = up.cross(up1); + p_offset = CLAMP(p_offset, 0.0, get_baked_length()); // PathFollower implement wrapping logic - if (axis.length_squared() < CMP_EPSILON2) { - axis = forward; - } else { - axis.normalize(); + Curve3D::Interval interval = _find_interval(p_offset); + return _sample_baked_tilt(interval); +} + +Vector3 Curve3D::sample_baked_up_vector(real_t p_offset, bool p_apply_tilt) const { + if (baked_cache_dirty) { + _bake(); } - return up.rotated(axis, up.angle_to(up1) * frac); + // Validate: Curve may not have baked up vectors. + ERR_FAIL_COND_V_MSG(!up_vector_enabled, Vector3(0, 1, 0), "No up vectors in Curve3D."); + + int count = baked_up_vector_cache.size(); + if (count == 1) { + return baked_up_vector_cache.get(0); + } + + p_offset = CLAMP(p_offset, 0.0, get_baked_length()); // PathFollower implement wrapping logic. + + Curve3D::Interval interval = _find_interval(p_offset); + return _sample_posture(interval, p_apply_tilt).get_column(1); } PackedVector3Array Curve3D::get_baked_points() const { @@ -1721,10 +1900,11 @@ Vector3 Curve3D::get_closest_point(const Vector3 &p_to_point) const { real_t nearest_dist = -1.0f; for (int i = 0; i < pc - 1; i++) { + const real_t interval = baked_dist_cache[i + 1] - baked_dist_cache[i]; Vector3 origin = r[i]; - Vector3 direction = (r[i + 1] - origin) / bake_interval; + Vector3 direction = (r[i + 1] - origin) / interval; - real_t d = CLAMP((p_to_point - origin).dot(direction), 0.0f, bake_interval); + real_t d = CLAMP((p_to_point - origin).dot(direction), 0.0f, interval); Vector3 proj = origin + direction * d; real_t dist = proj.distance_squared_to(p_to_point); @@ -1757,13 +1937,16 @@ real_t Curve3D::get_closest_offset(const Vector3 &p_to_point) const { real_t nearest = 0.0f; real_t nearest_dist = -1.0f; - real_t offset = 0.0f; + real_t offset; for (int i = 0; i < pc - 1; i++) { + offset = baked_dist_cache[i]; + + const real_t interval = baked_dist_cache[i + 1] - baked_dist_cache[i]; Vector3 origin = r[i]; - Vector3 direction = (r[i + 1] - origin) / bake_interval; + Vector3 direction = (r[i + 1] - origin) / interval; - real_t d = CLAMP((p_to_point - origin).dot(direction), 0.0f, bake_interval); + real_t d = CLAMP((p_to_point - origin).dot(direction), 0.0f, interval); Vector3 proj = origin + direction * d; real_t dist = proj.distance_squared_to(p_to_point); @@ -1772,8 +1955,6 @@ real_t Curve3D::get_closest_offset(const Vector3 &p_to_point) const { nearest = offset + d; nearest_dist = dist; } - - offset += bake_interval; } return nearest; @@ -1839,7 +2020,8 @@ void Curve3D::_set_data(const Dictionary &p_data) { points.write[i].tilt = rt[i]; } - baked_cache_dirty = true; + mark_dirty(); + notify_property_list_changed(); } PackedVector3Array Curve3D::tessellate(int p_max_stages, real_t p_tolerance) const { @@ -1877,6 +2059,50 @@ PackedVector3Array Curve3D::tessellate(int p_max_stages, real_t p_tolerance) con return tess; } +Vector<RBMap<real_t, Vector3>> Curve3D::_tessellate_even_length(int p_max_stages, real_t p_length) const { + Vector<RBMap<real_t, Vector3>> midpoints; + ERR_FAIL_COND_V_MSG(points.size() < 2, midpoints, "Curve must have at least 2 control point"); + + midpoints.resize(points.size() - 1); + + for (int i = 0; i < points.size() - 1; i++) { + _bake_segment3d_even_length(midpoints.write[i], 0, 1, points[i].position, points[i].out, points[i + 1].position, points[i + 1].in, 0, p_max_stages, p_length); + } + return midpoints; +} + +PackedVector3Array Curve3D::tessellate_even_length(int p_max_stages, real_t p_length) const { + PackedVector3Array tess; + + Vector<RBMap<real_t, Vector3>> midpoints = _tessellate_even_length(p_max_stages, p_length); + if (midpoints.size() == 0) { + return tess; + } + + int pc = 1; + for (int i = 0; i < points.size() - 1; i++) { + pc++; + pc += midpoints[i].size(); + } + + tess.resize(pc); + Vector3 *bpw = tess.ptrw(); + bpw[0] = points[0].position; + int pidx = 0; + + for (int i = 0; i < points.size() - 1; i++) { + for (const KeyValue<real_t, Vector3> &E : midpoints[i]) { + pidx++; + bpw[pidx] = E.value; + } + + pidx++; + bpw[pidx] = points[i + 1].position; + } + + return tess; +} + bool Curve3D::_set(const StringName &p_name, const Variant &p_value) { Vector<String> components = String(p_name).split("/", true, 2); if (components.size() >= 2 && components[0].begins_with("point_") && components[0].trim_prefix("point_").is_valid_int()) { @@ -1948,7 +2174,7 @@ void Curve3D::_get_property_list(List<PropertyInfo> *p_list) const { void Curve3D::_bind_methods() { ClassDB::bind_method(D_METHOD("get_point_count"), &Curve3D::get_point_count); ClassDB::bind_method(D_METHOD("set_point_count", "count"), &Curve3D::set_point_count); - ClassDB::bind_method(D_METHOD("add_point", "position", "in", "out", "at_position"), &Curve3D::add_point, DEFVAL(Vector3()), DEFVAL(Vector3()), DEFVAL(-1)); + ClassDB::bind_method(D_METHOD("add_point", "position", "in", "out", "index"), &Curve3D::add_point, DEFVAL(Vector3()), DEFVAL(Vector3()), DEFVAL(-1)); ClassDB::bind_method(D_METHOD("set_point_position", "idx", "position"), &Curve3D::set_point_position); ClassDB::bind_method(D_METHOD("get_point_position", "idx"), &Curve3D::get_point_position); ClassDB::bind_method(D_METHOD("set_point_tilt", "idx", "tilt"), &Curve3D::set_point_tilt); @@ -1959,8 +2185,8 @@ void Curve3D::_bind_methods() { ClassDB::bind_method(D_METHOD("get_point_out", "idx"), &Curve3D::get_point_out); ClassDB::bind_method(D_METHOD("remove_point", "idx"), &Curve3D::remove_point); ClassDB::bind_method(D_METHOD("clear_points"), &Curve3D::clear_points); - ClassDB::bind_method(D_METHOD("interpolate", "idx", "t"), &Curve3D::interpolate); - ClassDB::bind_method(D_METHOD("interpolatef", "fofs"), &Curve3D::interpolatef); + ClassDB::bind_method(D_METHOD("sample", "idx", "t"), &Curve3D::sample); + ClassDB::bind_method(D_METHOD("samplef", "fofs"), &Curve3D::samplef); //ClassDB::bind_method(D_METHOD("bake","subdivs"),&Curve3D::bake,DEFVAL(10)); ClassDB::bind_method(D_METHOD("set_bake_interval", "distance"), &Curve3D::set_bake_interval); ClassDB::bind_method(D_METHOD("get_bake_interval"), &Curve3D::get_bake_interval); @@ -1968,17 +2194,19 @@ void Curve3D::_bind_methods() { ClassDB::bind_method(D_METHOD("is_up_vector_enabled"), &Curve3D::is_up_vector_enabled); ClassDB::bind_method(D_METHOD("get_baked_length"), &Curve3D::get_baked_length); - ClassDB::bind_method(D_METHOD("interpolate_baked", "offset", "cubic"), &Curve3D::interpolate_baked, DEFVAL(false)); - ClassDB::bind_method(D_METHOD("interpolate_baked_up_vector", "offset", "apply_tilt"), &Curve3D::interpolate_baked_up_vector, DEFVAL(false)); + ClassDB::bind_method(D_METHOD("sample_baked", "offset", "cubic"), &Curve3D::sample_baked, DEFVAL(0.0), DEFVAL(false)); + ClassDB::bind_method(D_METHOD("sample_baked_with_rotation", "offset", "cubic", "apply_tilt"), &Curve3D::sample_baked_with_rotation, DEFVAL(0.0), DEFVAL(false), DEFVAL(false)); + ClassDB::bind_method(D_METHOD("sample_baked_up_vector", "offset", "apply_tilt"), &Curve3D::sample_baked_up_vector, DEFVAL(false)); ClassDB::bind_method(D_METHOD("get_baked_points"), &Curve3D::get_baked_points); ClassDB::bind_method(D_METHOD("get_baked_tilts"), &Curve3D::get_baked_tilts); ClassDB::bind_method(D_METHOD("get_baked_up_vectors"), &Curve3D::get_baked_up_vectors); ClassDB::bind_method(D_METHOD("get_closest_point", "to_point"), &Curve3D::get_closest_point); ClassDB::bind_method(D_METHOD("get_closest_offset", "to_point"), &Curve3D::get_closest_offset); ClassDB::bind_method(D_METHOD("tessellate", "max_stages", "tolerance_degrees"), &Curve3D::tessellate, DEFVAL(5), DEFVAL(4)); + ClassDB::bind_method(D_METHOD("tessellate_even_length", "max_stages", "tolerance_length"), &Curve3D::tessellate_even_length, DEFVAL(5), DEFVAL(0.2)); ClassDB::bind_method(D_METHOD("_get_data"), &Curve3D::_get_data); - ClassDB::bind_method(D_METHOD("_set_data"), &Curve3D::_set_data); + ClassDB::bind_method(D_METHOD("_set_data", "data"), &Curve3D::_set_data); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "bake_interval", PROPERTY_HINT_RANGE, "0.01,512,0.01"), "set_bake_interval", "get_bake_interval"); ADD_PROPERTY(PropertyInfo(Variant::INT, "_data", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR | PROPERTY_USAGE_INTERNAL), "_set_data", "_get_data"); |