diff options
| -rw-r--r-- | doc/classes/Curve3D.xml | 9 | ||||
| -rw-r--r-- | doc/classes/PathFollow3D.xml | 13 | ||||
| -rw-r--r-- | scene/3d/path_3d.cpp | 177 | ||||
| -rw-r--r-- | scene/3d/path_3d.h | 7 | ||||
| -rw-r--r-- | scene/resources/curve.cpp | 395 | ||||
| -rw-r--r-- | scene/resources/curve.h | 17 |
6 files changed, 352 insertions, 266 deletions
diff --git a/doc/classes/Curve3D.xml b/doc/classes/Curve3D.xml index 3e4e05f51a..63134417b1 100644 --- a/doc/classes/Curve3D.xml +++ b/doc/classes/Curve3D.xml @@ -132,6 +132,15 @@ If the curve has no up vectors, the function sends an error to the console, and returns [code](0, 1, 0)[/code]. </description> </method> + <method name="sample_baked_with_rotation" qualifiers="const"> + <return type="Transform3D" /> + <param index="0" name="offset" type="float" /> + <param index="1" name="cubic" type="bool" default="false" /> + <param index="2" name="apply_tilt" type="bool" default="false" /> + <description> + Similar with [code]interpolate_baked()[/code]. The the return value is [code]Transform3D[/code], with [code]origin[/code] as point position, [code]basis.x[/code] as sideway vector, [code]basis.y[/code] as up vector, [code]basis.z[/code] as forward vector. When the curve length is 0, there is no reasonable way to caculate the rotation, all vectors aligned with global space axes. + </description> + </method> <method name="samplef" qualifiers="const"> <return type="Vector3" /> <param index="0" name="fofs" type="float" /> diff --git a/doc/classes/PathFollow3D.xml b/doc/classes/PathFollow3D.xml index ba7207be8f..fa7580b7b6 100644 --- a/doc/classes/PathFollow3D.xml +++ b/doc/classes/PathFollow3D.xml @@ -9,6 +9,16 @@ </description> <tutorials> </tutorials> + <methods> + <method name="correct_posture" qualifiers="static"> + <return type="Transform3D" /> + <param index="0" name="transform" type="Transform3D" /> + <param index="1" name="rotation_mode" type="int" enum="PathFollow3D.RotationMode" /> + <description> + Correct the [code]transform[/code]. [code]rotation_mode[/code] implicitly specifies how posture (forward, up and sideway direction) is caculated. + </description> + </method> + </methods> <members> <member name="cubic_interp" type="bool" setter="set_cubic_interpolation" getter="get_cubic_interpolation" default="true"> If [code]true[/code], the position between two cached points is interpolated cubically, and linearly otherwise. @@ -30,6 +40,9 @@ <member name="rotation_mode" type="int" setter="set_rotation_mode" getter="get_rotation_mode" enum="PathFollow3D.RotationMode" default="3"> Allows or forbids rotation on one or more axes, depending on the [enum RotationMode] constants being used. </member> + <member name="tilt_enabled" type="bool" setter="set_tilt_enabled" getter="is_tilt_enabled" default="true"> + If [code]true[/code], the tilt property of [Curve3D] takes effect. + </member> <member name="v_offset" type="float" setter="set_v_offset" getter="get_v_offset" default="0.0"> The node's offset perpendicular to the curve. </member> diff --git a/scene/3d/path_3d.cpp b/scene/3d/path_3d.cpp index ab4cba86fb..02ab297d8e 100644 --- a/scene/3d/path_3d.cpp +++ b/scene/3d/path_3d.cpp @@ -182,125 +182,31 @@ void PathFollow3D::_update_transform(bool p_update_xyz_rot) { if (bl == 0.0) { return; } - real_t bi = c->get_bake_interval(); - real_t o_next = progress + bi; - real_t o_prev = progress - bi; - - if (loop) { - o_next = Math::fposmod(o_next, bl); - o_prev = Math::fposmod(o_prev, bl); - } else if (rotation_mode == ROTATION_ORIENTED) { - if (o_next >= bl) { - o_next = bl; - } - if (o_prev <= 0) { - o_prev = 0; - } - } - - Vector3 pos = c->sample_baked(progress, cubic); - Transform3D t = get_transform(); - // Vector3 pos_offset = Vector3(h_offset, v_offset, 0); not used in all cases - // will be replaced by "Vector3(h_offset, v_offset, 0)" where it was formerly used - - if (rotation_mode == ROTATION_ORIENTED) { - Vector3 forward = c->sample_baked(o_next, cubic) - pos; - - // Try with the previous position - if (forward.length_squared() < CMP_EPSILON2) { - forward = pos - c->sample_baked(o_prev, cubic); - } - - if (forward.length_squared() < CMP_EPSILON2) { - forward = Vector3(0, 0, 1); - } else { - forward.normalize(); - } - - Vector3 up = c->sample_baked_up_vector(progress, true); - if (o_next < progress) { - Vector3 up1 = c->sample_baked_up_vector(o_next, true); - Vector3 axis = up.cross(up1); - - if (axis.length_squared() < CMP_EPSILON2) { - axis = forward; - } else { - axis.normalize(); - } - - up.rotate(axis, up.angle_to(up1) * 0.5f); - } - - Vector3 scale = t.basis.get_scale(); - Vector3 sideways = up.cross(forward).normalized(); - up = forward.cross(sideways).normalized(); - - t.basis.set_columns(sideways, up, forward); - t.basis.scale_local(scale); - - t.origin = pos + sideways * h_offset + up * v_offset; - } else if (rotation_mode != ROTATION_NONE) { - // perform parallel transport - // - // see C. Dougan, The Parallel Transport Frame, Game Programming Gems 2 for example - // for a discussion about why not Frenet frame. + Transform3D t; + if (rotation_mode == ROTATION_NONE) { + Vector3 pos = c->sample_baked(progress, cubic); t.origin = pos; - if (p_update_xyz_rot && prev_offset != progress) { // Only update rotation if some parameter has changed - i.e. not on addition to scene tree. - real_t sample_distance = bi * 0.01; - Vector3 t_prev_pos_a = c->sample_baked(prev_offset - sample_distance, cubic); - Vector3 t_prev_pos_b = c->sample_baked(prev_offset + sample_distance, cubic); - Vector3 t_cur_pos_a = c->sample_baked(progress - sample_distance, cubic); - Vector3 t_cur_pos_b = c->sample_baked(progress + sample_distance, cubic); - Vector3 t_prev = (t_prev_pos_a - t_prev_pos_b).normalized(); - Vector3 t_cur = (t_cur_pos_a - t_cur_pos_b).normalized(); - - Vector3 axis = t_prev.cross(t_cur); - real_t dot = t_prev.dot(t_cur); - real_t angle = Math::acos(CLAMP(dot, -1, 1)); - - if (likely(!Math::is_zero_approx(angle))) { - if (rotation_mode == ROTATION_Y) { - // assuming we're referring to global Y-axis. is this correct? - axis.x = 0; - axis.z = 0; - } else if (rotation_mode == ROTATION_XY) { - axis.z = 0; - } else if (rotation_mode == ROTATION_XYZ) { - // all components are allowed - } + } else { + t = c->sample_baked_with_rotation(progress, cubic, false); + Vector3 forward = t.basis.get_column(2); // Retain tangent for applying tilt + t = PathFollow3D::correct_posture(t, rotation_mode); - if (likely(!Math::is_zero_approx(axis.length()))) { - t.rotate_basis(axis.normalized(), angle); - } - } + // Apply tilt *after* correct_posture + if (tilt_enabled) { + const real_t tilt = c->sample_baked_tilt(progress); - // do the additional tilting - real_t tilt_angle = c->sample_baked_tilt(progress); - Vector3 tilt_axis = t_cur; // not sure what tilt is supposed to do, is this correct?? - - if (likely(!Math::is_zero_approx(Math::abs(tilt_angle)))) { - if (rotation_mode == ROTATION_Y) { - tilt_axis.x = 0; - tilt_axis.z = 0; - } else if (rotation_mode == ROTATION_XY) { - tilt_axis.z = 0; - } else if (rotation_mode == ROTATION_XYZ) { - // all components are allowed - } - - if (likely(!Math::is_zero_approx(tilt_axis.length()))) { - t.rotate_basis(tilt_axis.normalized(), tilt_angle); - } - } + const Basis twist(forward, tilt); + t.basis = twist * t.basis; } - - t.translate_local(Vector3(h_offset, v_offset, 0)); - } else { - t.origin = pos + Vector3(h_offset, v_offset, 0); } + Vector3 scale = get_transform().basis.get_scale(); + + t.translate_local(Vector3(h_offset, v_offset, 0)); + t.basis.scale_local(scale); + set_transform(t); } @@ -358,6 +264,38 @@ PackedStringArray PathFollow3D::get_configuration_warnings() const { return warnings; } +Transform3D PathFollow3D::correct_posture(Transform3D p_transform, PathFollow3D::RotationMode p_rotation_mode) { + Transform3D t = p_transform; + + // Modify frame according to rotation mode. + if (p_rotation_mode == PathFollow3D::ROTATION_NONE) { + // Clear rotation. + t.basis = Basis(); + } else if (p_rotation_mode == PathFollow3D::ROTATION_ORIENTED) { + // Y-axis always straight up. + Vector3 up(0.0, 1.0, 0.0); + Vector3 forward = t.basis.get_column(2); + + t.basis = Basis::looking_at(-forward, up); + } else { + // Lock some euler axes. + Vector3 euler = t.basis.get_euler_normalized(EulerOrder::YXZ); + if (p_rotation_mode == PathFollow3D::ROTATION_Y) { + // Only Y-axis allowed. + euler[0] = 0; + euler[2] = 0; + } else if (p_rotation_mode == PathFollow3D::ROTATION_XY) { + // XY allowed. + euler[2] = 0; + } + + Basis locked = Basis::from_euler(euler, EulerOrder::YXZ); + t.basis = locked; + } + + return t; +} + void PathFollow3D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_progress", "progress"), &PathFollow3D::set_progress); ClassDB::bind_method(D_METHOD("get_progress"), &PathFollow3D::get_progress); @@ -380,6 +318,11 @@ void PathFollow3D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_loop", "loop"), &PathFollow3D::set_loop); ClassDB::bind_method(D_METHOD("has_loop"), &PathFollow3D::has_loop); + ClassDB::bind_method(D_METHOD("set_tilt_enabled", "enabled"), &PathFollow3D::set_tilt_enabled); + ClassDB::bind_method(D_METHOD("is_tilt_enabled"), &PathFollow3D::is_tilt_enabled); + + ClassDB::bind_static_method("PathFollow3D", D_METHOD("correct_posture", "transform", "rotation_mode"), &PathFollow3D::correct_posture); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "progress", PROPERTY_HINT_RANGE, "0,10000,0.01,or_less,or_greater,suffix:m"), "set_progress", "get_progress"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "progress_ratio", PROPERTY_HINT_RANGE, "0,1,0.0001,or_less,or_greater", PROPERTY_USAGE_EDITOR), "set_progress_ratio", "get_progress_ratio"); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "h_offset", PROPERTY_HINT_NONE, "suffix:m"), "set_h_offset", "get_h_offset"); @@ -387,6 +330,7 @@ void PathFollow3D::_bind_methods() { ADD_PROPERTY(PropertyInfo(Variant::INT, "rotation_mode", PROPERTY_HINT_ENUM, "None,Y,XY,XYZ,Oriented"), "set_rotation_mode", "get_rotation_mode"); ADD_PROPERTY(PropertyInfo(Variant::BOOL, "cubic_interp"), "set_cubic_interpolation", "get_cubic_interpolation"); ADD_PROPERTY(PropertyInfo(Variant::BOOL, "loop"), "set_loop", "has_loop"); + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "tilt_enabled"), "set_tilt_enabled", "is_tilt_enabled"); BIND_ENUM_CONSTANT(ROTATION_NONE); BIND_ENUM_CONSTANT(ROTATION_Y); @@ -397,7 +341,6 @@ void PathFollow3D::_bind_methods() { void PathFollow3D::set_progress(real_t p_progress) { ERR_FAIL_COND(!isfinite(p_progress)); - prev_offset = progress; progress = p_progress; if (path) { @@ -409,8 +352,6 @@ void PathFollow3D::set_progress(real_t p_progress) { if (!Math::is_zero_approx(p_progress) && Math::is_zero_approx(progress)) { progress = path_length; } - } else { - progress = CLAMP(progress, 0, path_length); } } @@ -476,3 +417,11 @@ void PathFollow3D::set_loop(bool p_loop) { bool PathFollow3D::has_loop() const { return loop; } + +void PathFollow3D::set_tilt_enabled(bool p_enable) { + tilt_enabled = p_enable; +} + +bool PathFollow3D::is_tilt_enabled() const { + return tilt_enabled; +} diff --git a/scene/3d/path_3d.h b/scene/3d/path_3d.h index b161b12185..9d5f694247 100644 --- a/scene/3d/path_3d.h +++ b/scene/3d/path_3d.h @@ -72,14 +72,16 @@ public: ROTATION_ORIENTED }; + static Transform3D correct_posture(Transform3D p_transform, PathFollow3D::RotationMode p_rotation_mode); + private: Path3D *path = nullptr; - real_t prev_offset = 0.0; // Offset during the last _update_transform. real_t progress = 0.0; real_t h_offset = 0.0; real_t v_offset = 0.0; bool cubic = true; bool loop = true; + bool tilt_enabled = true; RotationMode rotation_mode = ROTATION_XYZ; void _update_transform(bool p_update_xyz_rot = true); @@ -106,6 +108,9 @@ public: void set_loop(bool p_loop); bool has_loop() const; + void set_tilt_enabled(bool p_enable); + bool is_tilt_enabled() const; + void set_rotation_mode(RotationMode p_rotation_mode); RotationMode get_rotation_mode() const; diff --git a/scene/resources/curve.cpp b/scene/resources/curve.cpp index eda9af9dde..0c36abc148 100644 --- a/scene/resources/curve.cpp +++ b/scene/resources/curve.cpp @@ -31,6 +31,7 @@ #include "curve.h" #include "core/core_string_names.h" +#include "core/math/math_funcs.h" const char *Curve::SIGNAL_RANGE_CHANGED = "range_changed"; @@ -1413,8 +1414,9 @@ 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_up_vector_cache.clear(); return; } @@ -1438,15 +1440,16 @@ void Curve3D::_bake() const { Vector3 position = points[0].position; real_t dist = 0.0; - List<Plane> pointlist; + List<Plane> pointlist; // Abuse Plane for (position, dist) List<real_t> distlist; // Start always from origin. pointlist.push_back(Plane(position, points[0].tilt)); distlist.push_back(0.0); + // Step 1: Sample points + const real_t step = 0.1; // At least 10 substeps ought to be enough? 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) { @@ -1461,7 +1464,7 @@ void Curve3D::_bake() const { if (d > bake_interval) { // OK! between P and NP there _has_ to be Something, let's go searching! - int iterations = 10; //lots of detail! + const int iterations = 10; // Lots of detail! real_t low = p; real_t hi = np; @@ -1496,76 +1499,135 @@ void Curve3D::_bake() const { 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; + if (d > CMP_EPSILON) { // Avoid the degenerate case of two very close points. + position = npp; + Plane post; + post.normal = position; + post.d = points[i + 1].tilt; - dist += d; + dist += d; - pointlist.push_back(post); - distlist.push_back(dist); + pointlist.push_back(post); + distlist.push_back(dist); + } } baked_max_ofs = dist; - baked_point_cache.resize(pointlist.size()); - Vector3 *w = baked_point_cache.ptrw(); - int idx = 0; + const int point_count = pointlist.size(); + { + baked_point_cache.resize(point_count); + Vector3 *w = baked_point_cache.ptrw(); - baked_tilt_cache.resize(pointlist.size()); - real_t *wt = baked_tilt_cache.ptrw(); + baked_tilt_cache.resize(point_count); + real_t *wt = baked_tilt_cache.ptrw(); - baked_up_vector_cache.resize(up_vector_enabled ? pointlist.size() : 0); - Vector3 *up_write = baked_up_vector_cache.ptrw(); + baked_dist_cache.resize(point_count); + real_t *wd = baked_dist_cache.ptrw(); - baked_dist_cache.resize(pointlist.size()); - real_t *wd = baked_dist_cache.ptrw(); + int idx = 0; + for (const Plane &E : pointlist) { + w[idx] = E.normal; + wt[idx] = E.d; + wd[idx] = distlist[idx]; - Vector3 sideways; - Vector3 up; - Vector3 forward; + idx++; + } + } + + if (!up_vector_enabled) { + baked_up_vector_cache.resize(0); + return; + } - Vector3 prev_sideways = Vector3(1, 0, 0); - Vector3 prev_up = Vector3(0, 1, 0); - Vector3 prev_forward = Vector3(0, 0, 1); + // 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. + { + PackedVector3Array forward_vectors; - for (const Plane &E : pointlist) { - w[idx] = E.normal; - wt[idx] = E.d; - wd[idx] = distlist[idx]; + baked_up_vector_cache.resize(point_count); + forward_vectors.resize(point_count); - if (!up_vector_enabled) { - idx++; - continue; + Vector3 *up_write = baked_up_vector_cache.ptrw(); + Vector3 *forward_write = forward_vectors.ptrw(); + + const Vector3 *points_ptr = baked_point_cache.ptr(); + + Basis frame; // X-right, Y-up, Z-forward. + Basis frame_prev; + + // Set the initial frame based on Y-up rule. + { + Vector3 up(0, 1, 0); + Vector3 forward = (points_ptr[1] - points_ptr[0]).normalized(); + if (forward.is_equal_approx(Vector3())) { + forward = Vector3(1, 0, 0); + } + + if (abs(forward.dot(up)) > 1.0 - UNIT_EPSILON) { + frame_prev = Basis::looking_at(-forward, up); + } else { + frame_prev = Basis::looking_at(-forward, Vector3(1, 0, 0)); + } + + up_write[0] = frame_prev.get_column(1); + forward_write[0] = frame_prev.get_column(2); } - 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 = (points_ptr[idx] - points_ptr[idx - 1]).normalized(); + if (forward.is_equal_approx(Vector3())) { + forward = frame_prev.get_column(2); + } - 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); + forward_write[idx] = frame.get_column(2); + + 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_write[0].dot(forward_write[point_count - 1]); + if (dot < 1.0 - 0.01) { // 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_write[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) < UNIT_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_write[idx] * sign, angle); - idx++; + up_write[idx] = twist.xform(up_write[idx]); + } + } + } } } @@ -1577,27 +1639,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; @@ -1617,9 +1667,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]; @@ -1630,114 +1698,150 @@ 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(); - } - - // 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 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"); - if (pc == 1) { - return baked_tilt_cache.get(0); - } + int idx = p_interval.idx; + real_t frac = p_interval.frac; const real_t *r = baked_tilt_cache.ptr(); - if (p_offset < 0) { - return r[0]; + 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"); } - if (p_offset >= baked_max_ofs) { - return r[pc - 1]; + + int idx = p_interval.idx; + real_t frac = p_interval.frac; + + Vector3 forward_begin; + Vector3 forward_end; + if (idx == 0) { + forward_begin = (baked_point_cache[1] - baked_point_cache[0]).normalized(); + forward_end = (baked_point_cache[1] - baked_point_cache[0]).normalized(); + } else { + forward_begin = (baked_point_cache[idx] - baked_point_cache[idx - 1]).normalized(); + forward_end = (baked_point_cache[idx + 1] - baked_point_cache[idx]).normalized(); } - 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; + Vector3 up_begin; + Vector3 up_end; + if (up_vector_enabled) { + const Vector3 *up_ptr = baked_up_vector_cache.ptr(); + up_begin = up_ptr[idx]; + up_end = up_ptr[idx + 1]; + } else { + up_begin = Vector3(0.0, 1.0, 0.0); + up_end = Vector3(0.0, 1.0, 0.0); } - real_t offset_begin = baked_dist_cache[idx]; - real_t offset_end = baked_dist_cache[idx + 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(); - 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 (!p_apply_tilt) { + return frame; + } - real_t frac = (p_offset - offset_begin) / idx_interval; + // Applying tilt. + const real_t tilt = _sample_baked_tilt(p_interval); + Vector3 forward = frame.get_column(2); - return Math::lerp(r[idx], r[idx + 1], (real_t)frac); + const Basis twist(forward, tilt); + return twist * frame; } -Vector3 Curve3D::sample_baked_up_vector(real_t p_offset, bool p_apply_tilt) const { +Vector3 Curve3D::sample_baked(real_t p_offset, bool p_cubic) 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. + int pc = baked_point_cache.size(); + ERR_FAIL_COND_V_MSG(pc == 0, Vector3(), "No points in Curve3D."); - if (count == 1) { - return baked_up_vector_cache.get(0); + if (pc == 1) { + return baked_point_cache[0]; } - 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; + Curve3D::Interval interval = _find_interval(p_offset); + return _sample_baked(interval, p_cubic); +} + +Transform3D Curve3D::sample_baked_with_rotation(real_t p_offset, bool p_cubic, bool p_apply_tilt) const { + if (baked_cache_dirty) { + _bake(); } - if (idx == count - 1) { - return p_apply_tilt ? r[idx].rotated((rp[idx] - rp[idx - 1]).normalized(), rt[idx]) : r[idx]; + // 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 (point_count == 1) { + Transform3D t; + t.origin = baked_point_cache.get(0); + ERR_FAIL_V_MSG(t, "Only 1 point in Curve3D."); } - real_t offset_begin = baked_dist_cache[idx]; - real_t offset_end = baked_dist_cache[idx + 1]; + p_offset = CLAMP(p_offset, 0.0, get_baked_length()); // PathFollower implement wrapping logic. - 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."); + // 0. Find interval for all sampling steps. + Curve3D::Interval interval = _find_interval(p_offset); - real_t frac = (p_offset - offset_begin) / idx_interval; + // 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); +} + +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(); + } + + // 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); } - 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 { @@ -2034,6 +2138,7 @@ void Curve3D::_bind_methods() { 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_with_rotation", "offset", "cubic", "apply_tilt"), &Curve3D::sample_baked_with_rotation, 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); diff --git a/scene/resources/curve.h b/scene/resources/curve.h index fa1d35aab1..d0c813f262 100644 --- a/scene/resources/curve.h +++ b/scene/resources/curve.h @@ -238,11 +238,6 @@ class Curve3D : public Resource { Vector<Point> points; - struct BakedPoint { - real_t ofs = 0.0; - Vector3 point; - }; - mutable bool baked_cache_dirty = false; mutable PackedVector3Array baked_point_cache; mutable Vector<real_t> baked_tilt_cache; @@ -254,6 +249,15 @@ class Curve3D : public Resource { void _bake() const; + struct Interval { + int idx; + real_t frac; + }; + Interval _find_interval(real_t p_offset) const; + Vector3 _sample_baked(Interval p_interval, bool p_cubic) const; + real_t _sample_baked_tilt(Interval p_interval) const; + Basis _sample_posture(Interval p_interval, bool p_apply_tilt = false) const; + real_t bake_interval = 0.2; bool up_vector_enabled = true; @@ -296,9 +300,10 @@ public: real_t get_baked_length() const; Vector3 sample_baked(real_t p_offset, bool p_cubic = false) const; + Transform3D sample_baked_with_rotation(real_t p_offset, bool p_cubic = false, bool p_apply_tilt = false) const; real_t sample_baked_tilt(real_t p_offset) const; Vector3 sample_baked_up_vector(real_t p_offset, bool p_apply_tilt = false) const; - PackedVector3Array get_baked_points() const; //useful for going through + PackedVector3Array get_baked_points() const; // Useful for going through. Vector<real_t> get_baked_tilts() const; //useful for going through PackedVector3Array get_baked_up_vectors() const; Vector3 get_closest_point(const Vector3 &p_to_point) const; |