diff options
Diffstat (limited to 'scene/resources/curve.cpp')
| -rw-r--r-- | scene/resources/curve.cpp | 904 |
1 files changed, 568 insertions, 336 deletions
diff --git a/scene/resources/curve.cpp b/scene/resources/curve.cpp index 49b78a091d..6fa0ebbf55 100644 --- a/scene/resources/curve.cpp +++ b/scene/resources/curve.cpp @@ -1,36 +1,37 @@ -/*************************************************************************/ -/* curve.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ -/* */ -/* Permission is hereby granted, free of charge, to any person obtaining */ -/* a copy of this software and associated documentation files (the */ -/* "Software"), to deal in the Software without restriction, including */ -/* without limitation the rights to use, copy, modify, merge, publish, */ -/* distribute, sublicense, and/or sell copies of the Software, and to */ -/* permit persons to whom the Software is furnished to do so, subject to */ -/* the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ -/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ -/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ -/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ -/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/*************************************************************************/ +/**************************************************************************/ +/* curve.cpp */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ #include "curve.h" #include "core/core_string_names.h" +#include "core/math/math_funcs.h" const char *Curve::SIGNAL_RANGE_CHANGED = "range_changed"; @@ -340,7 +341,7 @@ 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 * / \ @@ -773,6 +774,35 @@ 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); + } +} + +Vector2 Curve2D::_calculate_tangent(const Vector2 &p_begin, const Vector2 &p_control_1, const Vector2 &p_control_2, const Vector2 &p_end, const real_t p_t) { + // Handle corner cases. + if (Math::is_zero_approx(p_t - 0.0f) && p_control_1.is_equal_approx(p_begin)) { + return (p_end - p_begin).normalized(); + } + + if (Math::is_zero_approx(p_t - 1.0f) && p_control_2.is_equal_approx(p_end)) { + return (p_end - p_begin).normalized(); + } + + return p_begin.bezier_derivative(p_control_1, p_control_2, p_end, p_t).normalized(); +} + void Curve2D::_bake() const { if (!baked_cache_dirty) { return; @@ -784,94 +814,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; + // Tessellate curve to (almost) even length segments + { + Vector<RBMap<real_t, Vector2>> midpoints = _tessellate_even_length(10, bake_interval); - 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; - } - - Vector2 npp = points[i].position.bezier_interpolate(points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position, np); - 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 = points[i].position.bezier_interpolate(points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position, mid); - 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); + baked_point_cache.resize(pc); + baked_dist_cache.resize(pc); + baked_forward_vector_cache.resize(pc); - position = npp; - dist += d; + Vector2 *bpw = baked_point_cache.ptrw(); + Vector2 *bfw = baked_forward_vector_cache.ptrw(); - pointlist.push_back(position); - distlist.push_back(dist); - } - - baked_max_ofs = dist; + // Collect positions and sample tilts and tangents for each baked points. + bpw[0] = points[0].position; + bfw[0] = _calculate_tangent(points[0].position, points[0].position + points[0].out, points[1].position + points[1].in, points[1].position, 0.0); + 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] = _calculate_tangent(points[i].position, points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position, E.key); + } - Vector2 *w = baked_point_cache.ptrw(); - real_t *wd = baked_dist_cache.ptrw(); + pidx++; + bpw[pidx] = points[i + 1].position; + bfw[pidx] = _calculate_tangent(points[i].position, points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position, 1.0); + } - 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]; } } @@ -883,27 +881,15 @@ real_t Curve2D::get_baked_length() const { return baked_max_ofs; } -Vector2 Curve2D::sample_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; @@ -923,9 +909,27 @@ Vector2 Curve2D::sample_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(), "Couldn't 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; +} + +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]; @@ -936,6 +940,72 @@ Vector2 Curve2D::sample_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(); @@ -974,10 +1044,11 @@ Vector2 Curve2D::get_closest_point(const Vector2 &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]; Vector2 origin = r[i]; - Vector2 direction = (r[i + 1] - origin) / bake_interval; + Vector2 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); Vector2 proj = origin + direction * d; real_t dist = proj.distance_squared_to(p_to_point); @@ -1013,10 +1084,13 @@ real_t Curve2D::get_closest_offset(const Vector2 &p_to_point) const { real_t offset = 0.0f; 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]; Vector2 origin = r[i]; - Vector2 direction = (r[i + 1] - origin) / bake_interval; + Vector2 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); Vector2 proj = origin + direction * d; real_t dist = proj.distance_squared_to(p_to_point); @@ -1025,8 +1099,6 @@ real_t Curve2D::get_closest_offset(const Vector2 &p_to_point) const { nearest = offset + d; nearest_dist = dist; } - - offset += bake_interval; } return nearest; @@ -1106,6 +1178,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()) { @@ -1183,11 +1299,13 @@ void Curve2D::_bind_methods() { 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("sample_baked", "offset", "cubic"), &Curve2D::sample_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", "data"), &Curve2D::_set_data); @@ -1361,6 +1479,35 @@ 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); + } +} + +Vector3 Curve3D::_calculate_tangent(const Vector3 &p_begin, const Vector3 &p_control_1, const Vector3 &p_control_2, const Vector3 &p_end, const real_t p_t) { + // Handle corner cases. + if (Math::is_zero_approx(p_t - 0.0f) && p_control_1.is_equal_approx(p_begin)) { + return (p_end - p_begin).normalized(); + } + + if (Math::is_zero_approx(p_t - 1.0f) && p_control_2.is_equal_approx(p_end)) { + return (p_end - p_begin).normalized(); + } + + return p_begin.bezier_derivative(p_control_1, p_control_2, p_end, p_t).normalized(); +} + void Curve3D::_bake() const { if (!baked_cache_dirty) { return; @@ -1372,8 +1519,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; } @@ -1384,10 +1533,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(); } @@ -1395,136 +1546,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: Tessellate 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] = _calculate_tangent(points[0].position, points[0].position + points[0].out, points[1].position + points[1].in, points[1].position, 0.0); + 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] = _calculate_tangent(points[i].position, points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position, E.key); + btw[pidx] = Math::lerp(points[i].tilt, points[i + 1].tilt, E.key); } - Vector3 npp = points[i].position.bezier_interpolate(points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position, np); - 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 = points[i].position.bezier_interpolate(points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position, mid); - 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] = _calculate_tangent(points[i].position, points[i].position + points[i].out, points[i + 1].position + points[i + 1].in, points[i + 1].position, 1.0); + 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 doesn'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]); + } + } + } } } @@ -1536,27 +1686,15 @@ real_t Curve3D::get_baked_length() const { return baked_max_ofs; } -Vector3 Curve3D::sample_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; @@ -1576,9 +1714,27 @@ Vector3 Curve3D::sample_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(), "Couldn't 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]; @@ -1589,114 +1745,142 @@ Vector3 Curve3D::sample_baked(real_t p_offset, bool p_cubic) const { } } -real_t Curve3D::sample_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, "Couldn't 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::sample_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); + + // 1. Sample position. + Vector3 pos = _sample_baked(interval, p_cubic); + + // 2. Sample rotation frame. + Basis frame = _sample_posture(interval, p_apply_tilt); + + 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(); } - real_t offset_begin = baked_dist_cache[idx]; - real_t offset_end = baked_dist_cache[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."); - 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), "Couldn't find baked segment."); + if (pc == 1) { + return baked_tilt_cache.get(0); + } - real_t frac = (p_offset - offset_begin) / idx_interval; + p_offset = CLAMP(p_offset, 0.0, get_baked_length()); // PathFollower implement wrapping logic - Vector3 forward = (rp[idx + 1] - rp[idx]).normalized(); - Vector3 up = r[idx]; - Vector3 up1 = r[idx + 1]; + Curve3D::Interval interval = _find_interval(p_offset); + return _sample_baked_tilt(interval); +} - 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]); +Vector3 Curve3D::sample_baked_up_vector(real_t p_offset, bool p_apply_tilt) const { + if (baked_cache_dirty) { + _bake(); } - Vector3 axis = up.cross(up1); + // 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."); - if (axis.length_squared() < CMP_EPSILON2) { - axis = forward; - } else { - axis.normalize(); + int count = baked_up_vector_cache.size(); + if (count == 1) { + return baked_up_vector_cache.get(0); } - return up.rotated(axis, up.angle_to(up1) * frac); + 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 { @@ -1744,10 +1928,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); @@ -1780,13 +1965,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); @@ -1795,8 +1983,6 @@ real_t Curve3D::get_closest_offset(const Vector3 &p_to_point) const { nearest = offset + d; nearest_dist = dist; } - - offset += bake_interval; } return nearest; @@ -1901,6 +2087,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()) { @@ -1992,7 +2222,8 @@ 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("sample_baked", "offset", "cubic"), &Curve3D::sample_baked, 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); @@ -2000,6 +2231,7 @@ void Curve3D::_bind_methods() { 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", "data"), &Curve3D::_set_data); |