From dc639d334a1e7ed81c25ce76cd986b2f49f85e65 Mon Sep 17 00:00:00 2001 From: danilo2205 Date: Fri, 18 May 2018 19:14:25 -0300 Subject: [PATCH] Up vector implementation and OrientedPathFollow. --- doc/classes/Curve2D.xml | 4 + doc/classes/Curve3D.xml | 30 ++- doc/classes/OrientedPathFollow.xml | 40 ++++ editor/icons/icon_oriented_path_follow.svg | 5 + scene/3d/path.cpp | 216 +++++++++++++++++++++ scene/3d/path.h | 43 ++++ scene/register_scene_types.cpp | 1 + scene/resources/curve.cpp | 125 ++++++++++++ scene/resources/curve.h | 6 + 9 files changed, 469 insertions(+), 1 deletion(-) create mode 100644 doc/classes/OrientedPathFollow.xml create mode 100644 editor/icons/icon_oriented_path_follow.svg diff --git a/doc/classes/Curve2D.xml b/doc/classes/Curve2D.xml index 71bdaff688f..26de8be42c8 100644 --- a/doc/classes/Curve2D.xml +++ b/doc/classes/Curve2D.xml @@ -55,6 +55,8 @@ + Returns the closest offset to [code]to_point[/code]. This offset is meant to be used in [method interpolate_baked]. + [code]to_point[/code] must be in this curve's local space. @@ -63,6 +65,8 @@ + Returns the closest point (in curve's local space) to [code]to_point[/code]. + [code]to_point[/code] must be in this curve's local space. diff --git a/doc/classes/Curve3D.xml b/doc/classes/Curve3D.xml index c012e2794e0..1355c74fafa 100644 --- a/doc/classes/Curve3D.xml +++ b/doc/classes/Curve3D.xml @@ -56,12 +56,22 @@ Returns the cache of tilts as a [RealArray]. + + + + + Returns the cache of up vectors as a [PoolVector3Array]. + If [member up_vector_enabled] is [code]false[/code], the cache will be empty. + + + Returns the closest offset to [code]to_point[/code]. This offset is meant to be used in one of the interpolate_baked* methods. + [code]to_point[/code] must be in this curve's local space. @@ -70,6 +80,8 @@ + Returns the closest point (in curve's local space) to [code]to_point[/code]. + [code]to_point[/code] must be in this curve's local space. @@ -140,6 +152,19 @@ Cubic interpolation tends to follow the curves better, but linear is faster (and often, precise enough). + + + + + + + + + Returns an up vector within the curve at position [code]offset[/code], where [code]offset[/code] is measured as a distance in 3D units along the curve. + To do that, it finds the two cached up vectors where the [code]offset[/code] lies between, then interpolates the values. If [code]apply_tilt[/code] is [code]true[/code], an interpolated tilt is applied to the interpolated up vector. + If the curve has no up vectors, the function sends an error to the console, and returns (0, 1, 0). + + @@ -200,7 +225,7 @@ Sets the tilt angle in radians for the point "idx". If the index is out of bounds, the function sends an error to the console. - The tilt controls the rotation along the look-at axis an object traveling the path would have. In the case of a curve controlling a [PathFollow], this tilt is an offset over the natural tilt the PathFollow calculates. + The tilt controls the rotation along the look-at axis an object traveling the path would have. In the case of a curve controlling a [PathFollow] or [OrientedPathFollow], this tilt is an offset over the natural tilt the [PathFollow] or [OrientedPathFollow] calculates. @@ -222,6 +247,9 @@ The distance in meters between two adjacent cached points. Changing it forces the cache to be recomputed the next time the [method get_baked_points] or [method get_baked_length] function is called. The smaller the distance, the more points in the cache and the more memory it will consume, so use with care. + + If [code]true[/code], the curve will bake up vectors used for orientation. See [OrientedPathFollow]. Changing it forces the cache to be recomputed. + diff --git a/doc/classes/OrientedPathFollow.xml b/doc/classes/OrientedPathFollow.xml new file mode 100644 index 00000000000..c32e545ff5d --- /dev/null +++ b/doc/classes/OrientedPathFollow.xml @@ -0,0 +1,40 @@ + + + + Oriented point sampler for a [Path]. + + + This node behaves like [PathFollow], except it uses its parent [Path] up vector information to enforce orientation. + Make sure to check if the curve of this node's parent [Path] has up vectors enabled. See [PathFollow] and [Curve3D] for further information. + + + + + + + + + + If [code]true[/code] the position between two cached points is interpolated cubically, and linearly otherwise. + The points along the [Curve3D] of the [Path] are precomputed before use, for faster calculations. The point at the requested offset is then calculated interpolating between two adjacent cached points. This may present a problem if the curve makes sharp turns, as the cached points may not follow the curve closely enough. + There are two answers to this problem: Either increase the number of cached points and increase memory consumption, or make a cubic interpolation between two points at the cost of (slightly) slower calculations. + + + The node's offset along the curve. + + + If [code]true[/code], any offset outside the path's length will wrap around, instead of stopping at the ends. Use it for cyclic paths. + + + The distance from the first vertex, measured in 3D units along the path. This sets this node's position to a point within the path. + + + The distance from the first vertex, considering 0.0 as the first vertex and 1.0 as the last. This is just another way of expressing the offset within the path, as the offset supplied is multiplied internally by the path's length. + + + The node's offset perpendicular to the curve. + + + + + diff --git a/editor/icons/icon_oriented_path_follow.svg b/editor/icons/icon_oriented_path_follow.svg new file mode 100644 index 00000000000..bd3f585e54e --- /dev/null +++ b/editor/icons/icon_oriented_path_follow.svg @@ -0,0 +1,5 @@ + + + + + diff --git a/scene/3d/path.cpp b/scene/3d/path.cpp index 154dcb4259c..9acaa156417 100644 --- a/scene/3d/path.cpp +++ b/scene/3d/path.cpp @@ -329,3 +329,219 @@ PathFollow::PathFollow() { cubic = true; loop = true; } + +////////////// + +void OrientedPathFollow::_update_transform() { + + if (!path) + return; + + Ref c = path->get_curve(); + if (!c.is_valid()) + return; + + int count = c->get_point_count(); + if (count < 2) + return; + + if (delta_offset == 0) { + return; + } + + float offset = get_offset(); + float bl = c->get_baked_length(); + float bi = c->get_bake_interval(); + float o = offset; + float o_next = offset + bi; + + if (has_loop()) { + o = Math::fposmod(o, bl); + o_next = Math::fposmod(o_next, bl); + } else if (o_next >= bl) { + o = bl - bi; + o_next = bl; + } + + bool cubic = get_cubic_interpolation(); + Vector3 pos = c->interpolate_baked(o, cubic); + Vector3 forward = c->interpolate_baked(o_next, cubic) - pos; + + if (forward.length_squared() < CMP_EPSILON2) + forward = Vector3(0, 0, 1); + else + forward.normalize(); + + Vector3 up = c->interpolate_baked_up_vector(o, true); + + if (o_next < o) { + Vector3 up1 = c->interpolate_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); + } + + Transform t = get_transform(); + Vector3 scale = t.basis.get_scale(); + + Vector3 sideways = up.cross(forward).normalized(); + up = forward.cross(sideways).normalized(); + + t.basis.set(sideways, up, forward); + t.basis.scale_local(scale); + + t.origin = pos + sideways * get_h_offset() + up * get_v_offset(); + + set_transform(t); +} + +void OrientedPathFollow::_notification(int p_what) { + + switch (p_what) { + + case NOTIFICATION_ENTER_TREE: { + + Node *parent = get_parent(); + if (parent) { + path = Object::cast_to(parent); + if (path) { + _update_transform(); + } + } + + } break; + case NOTIFICATION_EXIT_TREE: { + + path = NULL; + } break; + } +} + +void OrientedPathFollow::set_cubic_interpolation(bool p_enable) { + + cubic = p_enable; +} + +bool OrientedPathFollow::get_cubic_interpolation() const { + + return cubic; +} + +void OrientedPathFollow::_validate_property(PropertyInfo &property) const { + + if (property.name == "offset") { + + float max = 10000; + if (path && path->get_curve().is_valid()) + max = path->get_curve()->get_baked_length(); + + property.hint_string = "0," + rtos(max) + ",0.01"; + } +} + +void OrientedPathFollow::_bind_methods() { + + ClassDB::bind_method(D_METHOD("set_offset", "offset"), &OrientedPathFollow::set_offset); + ClassDB::bind_method(D_METHOD("get_offset"), &OrientedPathFollow::get_offset); + + ClassDB::bind_method(D_METHOD("set_h_offset", "h_offset"), &OrientedPathFollow::set_h_offset); + ClassDB::bind_method(D_METHOD("get_h_offset"), &OrientedPathFollow::get_h_offset); + + ClassDB::bind_method(D_METHOD("set_v_offset", "v_offset"), &OrientedPathFollow::set_v_offset); + ClassDB::bind_method(D_METHOD("get_v_offset"), &OrientedPathFollow::get_v_offset); + + ClassDB::bind_method(D_METHOD("set_unit_offset", "unit_offset"), &OrientedPathFollow::set_unit_offset); + ClassDB::bind_method(D_METHOD("get_unit_offset"), &OrientedPathFollow::get_unit_offset); + + ClassDB::bind_method(D_METHOD("set_cubic_interpolation", "enable"), &OrientedPathFollow::set_cubic_interpolation); + ClassDB::bind_method(D_METHOD("get_cubic_interpolation"), &OrientedPathFollow::get_cubic_interpolation); + + ClassDB::bind_method(D_METHOD("set_loop", "loop"), &OrientedPathFollow::set_loop); + ClassDB::bind_method(D_METHOD("has_loop"), &OrientedPathFollow::has_loop); + + ADD_PROPERTY(PropertyInfo(Variant::REAL, "offset", PROPERTY_HINT_RANGE, "0,10000,0.01"), "set_offset", "get_offset"); + ADD_PROPERTY(PropertyInfo(Variant::REAL, "unit_offset", PROPERTY_HINT_RANGE, "0,1,0.0001", PROPERTY_USAGE_EDITOR), "set_unit_offset", "get_unit_offset"); + ADD_PROPERTY(PropertyInfo(Variant::REAL, "h_offset"), "set_h_offset", "get_h_offset"); + ADD_PROPERTY(PropertyInfo(Variant::REAL, "v_offset"), "set_v_offset", "get_v_offset"); + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "cubic_interp"), "set_cubic_interpolation", "get_cubic_interpolation"); + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "loop"), "set_loop", "has_loop"); +} + +void OrientedPathFollow::set_offset(float p_offset) { + delta_offset = p_offset - offset; + offset = p_offset; + + if (path) + _update_transform(); + _change_notify("offset"); + _change_notify("unit_offset"); +} + +void OrientedPathFollow::set_h_offset(float p_h_offset) { + + h_offset = p_h_offset; + if (path) + _update_transform(); +} + +float OrientedPathFollow::get_h_offset() const { + + return h_offset; +} + +void OrientedPathFollow::set_v_offset(float p_v_offset) { + + v_offset = p_v_offset; + if (path) + _update_transform(); +} + +float OrientedPathFollow::get_v_offset() const { + + return v_offset; +} + +float OrientedPathFollow::get_offset() const { + + return offset; +} + +void OrientedPathFollow::set_unit_offset(float p_unit_offset) { + + if (path && path->get_curve().is_valid() && path->get_curve()->get_baked_length()) + set_offset(p_unit_offset * path->get_curve()->get_baked_length()); +} + +float OrientedPathFollow::get_unit_offset() const { + + if (path && path->get_curve().is_valid() && path->get_curve()->get_baked_length()) + return get_offset() / path->get_curve()->get_baked_length(); + else + return 0; +} + +void OrientedPathFollow::set_loop(bool p_loop) { + + loop = p_loop; +} + +bool OrientedPathFollow::has_loop() const { + + return loop; +} + +OrientedPathFollow::OrientedPathFollow() { + + offset = 0; + delta_offset = 0; + h_offset = 0; + v_offset = 0; + path = NULL; + cubic = true; + loop = true; +} diff --git a/scene/3d/path.h b/scene/3d/path.h index 2ed686ac3c7..f73bf17dfe5 100644 --- a/scene/3d/path.h +++ b/scene/3d/path.h @@ -111,4 +111,47 @@ public: VARIANT_ENUM_CAST(PathFollow::RotationMode); +class OrientedPathFollow : public Spatial { + + GDCLASS(OrientedPathFollow, Spatial); + +private: + Path *path; + real_t delta_offset; // change in offset since last _update_transform + real_t offset; + real_t h_offset; + real_t v_offset; + bool cubic; + bool loop; + + void _update_transform(); + +protected: + virtual void _validate_property(PropertyInfo &property) const; + + void _notification(int p_what); + static void _bind_methods(); + +public: + void set_offset(float p_offset); + float get_offset() const; + + void set_h_offset(float p_h_offset); + float get_h_offset() const; + + void set_v_offset(float p_v_offset); + float get_v_offset() const; + + void set_unit_offset(float p_unit_offset); + float get_unit_offset() const; + + void set_loop(bool p_loop); + bool has_loop() const; + + void set_cubic_interpolation(bool p_enable); + bool get_cubic_interpolation() const; + + OrientedPathFollow(); +}; + #endif // PATH_H diff --git a/scene/register_scene_types.cpp b/scene/register_scene_types.cpp index 7533fa5f6ce..2f3d4df329e 100644 --- a/scene/register_scene_types.cpp +++ b/scene/register_scene_types.cpp @@ -404,6 +404,7 @@ void register_scene_types() { ClassDB::register_class(); ClassDB::register_class(); ClassDB::register_class(); + ClassDB::register_class(); ClassDB::register_class(); ClassDB::register_class(); ClassDB::register_class(); diff --git a/scene/resources/curve.cpp b/scene/resources/curve.cpp index 4ec1e8973d1..7f902fc982e 100644 --- a/scene/resources/curve.cpp +++ b/scene/resources/curve.cpp @@ -1169,6 +1169,7 @@ void Curve3D::_bake() const { if (points.size() == 0) { baked_point_cache.resize(0); baked_tilt_cache.resize(0); + baked_up_vector_cache.resize(0); return; } @@ -1178,6 +1179,14 @@ void Curve3D::_bake() const { baked_point_cache.set(0, points[0].pos); baked_tilt_cache.resize(1); baked_tilt_cache.set(0, points[0].tilt); + + if (up_vector_enabled) { + + baked_up_vector_cache.resize(1); + baked_up_vector_cache.set(0, Vector3(0, 1, 0)); + } else + baked_up_vector_cache.resize(0); + return; } @@ -1247,10 +1256,51 @@ void Curve3D::_bake() const { baked_tilt_cache.resize(pointlist.size()); PoolRealArray::Write wt = baked_tilt_cache.write(); + baked_up_vector_cache.resize(up_vector_enabled ? pointlist.size() : 0); + PoolVector3Array::Write up_write = baked_up_vector_cache.write(); + + Vector3 sideways; + Vector3 up; + Vector3 forward; + + Vector3 prev_sideways = Vector3(1, 0, 0); + Vector3 prev_up = Vector3(0, 1, 0); + Vector3 prev_forward = Vector3(0, 0, 1); + for (List::Element *E = pointlist.front(); E; E = E->next()) { w[idx] = E->get().normal; wt[idx] = E->get().d; + + if (!up_vector_enabled) { + idx++; + continue; + } + + forward = idx > 0 ? (w[idx] - w[idx - 1]).normalized() : prev_forward; + + float y_dot = prev_up.dot(forward); + + 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(); + } + + if (idx == 1) + up_write[0] = up; + + up_write[idx] = up; + + prev_sideways = sideways; + prev_up = up; + prev_forward = forward; + idx++; } } @@ -1343,6 +1393,53 @@ float Curve3D::interpolate_baked_tilt(float p_offset) const { return Math::lerp(r[idx], r[idx + 1], frac); } +Vector3 Curve3D::interpolate_baked_up_vector(float p_offset, 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(); + if (count == 0) { + ERR_EXPLAIN("No up vectors in Curve3D"); + ERR_FAIL_COND_V(count == 0, Vector3(0, 1, 0)); + } + + if (count == 1) + return baked_up_vector_cache.get(0); + + PoolVector3Array::Read r = baked_up_vector_cache.read(); + PoolVector3Array::Read rp = baked_point_cache.read(); + PoolRealArray::Read rt = baked_tilt_cache.read(); + + float offset = CLAMP(p_offset, 0.0f, baked_max_ofs); + + int idx = Math::floor((double)offset / (double)bake_interval); + float frac = Math::fmod(offset, bake_interval) / bake_interval; + + if (idx == count - 1) + return p_apply_tilt ? r[idx].rotated((rp[idx] - rp[idx - 1]).normalized(), rt[idx]) : r[idx]; + + Vector3 forward = (rp[idx + 1] - rp[idx]).normalized(); + Vector3 up = r[idx]; + Vector3 up1 = r[idx + 1]; + + 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 axis = up.cross(up1); + + if (axis.length_squared() < CMP_EPSILON2) + axis = forward; + else + axis.normalize(); + + return up.rotated(axis, up.angle_to(up1) * frac); +} + PoolVector3Array Curve3D::get_baked_points() const { if (baked_cache_dirty) @@ -1359,6 +1456,14 @@ PoolRealArray Curve3D::get_baked_tilts() const { return baked_tilt_cache; } +PoolVector3Array Curve3D::get_baked_up_vectors() const { + + if (baked_cache_dirty) + _bake(); + + return baked_up_vector_cache; +} + Vector3 Curve3D::get_closest_point(const Vector3 &p_to_point) const { // Brute force method @@ -1452,6 +1557,18 @@ float Curve3D::get_bake_interval() const { return bake_interval; } +void Curve3D::set_up_vector_enabled(bool p_enable) { + + up_vector_enabled = p_enable; + baked_cache_dirty = true; + emit_signal(CoreStringNames::get_singleton()->changed); +} + +bool Curve3D::is_up_vector_enabled() const { + + return up_vector_enabled; +} + Dictionary Curve3D::_get_data() const { Dictionary dc; @@ -1563,11 +1680,15 @@ void Curve3D::_bind_methods() { //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); + ClassDB::bind_method(D_METHOD("set_up_vector_enabled", "enable"), &Curve3D::set_up_vector_enabled); + 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("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)); @@ -1577,6 +1698,9 @@ void Curve3D::_bind_methods() { ADD_PROPERTY(PropertyInfo(Variant::REAL, "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_NOEDITOR | PROPERTY_USAGE_INTERNAL), "_set_data", "_get_data"); + + ADD_GROUP("Up Vector", "up_vector_"); + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "up_vector_enabled"), "set_up_vector_enabled", "is_up_vector_enabled"); } Curve3D::Curve3D() { @@ -1586,4 +1710,5 @@ Curve3D::Curve3D() { add_point(Vector3(0,2,0)); add_point(Vector3(0,3,5));*/ bake_interval = 0.2; + up_vector_enabled = true; } diff --git a/scene/resources/curve.h b/scene/resources/curve.h index 492eb05d1e9..9cb12a4345e 100644 --- a/scene/resources/curve.h +++ b/scene/resources/curve.h @@ -232,11 +232,13 @@ class Curve3D : public Resource { mutable bool baked_cache_dirty; mutable PoolVector3Array baked_point_cache; mutable PoolRealArray baked_tilt_cache; + mutable PoolVector3Array baked_up_vector_cache; mutable float baked_max_ofs; void _bake() const; float bake_interval; + bool up_vector_enabled; void _bake_segment3d(Map &r_bake, float p_begin, float 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, float p_tol) const; Dictionary _get_data() const; @@ -264,12 +266,16 @@ public: void set_bake_interval(float p_tolerance); float get_bake_interval() const; + void set_up_vector_enabled(bool p_enable); + bool is_up_vector_enabled() const; float get_baked_length() const; Vector3 interpolate_baked(float p_offset, bool p_cubic = false) const; float interpolate_baked_tilt(float p_offset) const; + Vector3 interpolate_baked_up_vector(float p_offset, bool p_apply_tilt = false) const; PoolVector3Array get_baked_points() const; //useful for going through PoolRealArray get_baked_tilts() const; //useful for going through + PoolVector3Array get_baked_up_vectors() const; Vector3 get_closest_point(const Vector3 &p_to_point) const; float get_closest_offset(const Vector3 &p_to_point) const;