5 files changed, 391 insertions, 0 deletions

diff --git a/scene/3d/path.cpp b/scene/3d/path.cpp
index 154dcb4259..9acaa15641 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<Curve3D> 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<Path>(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 2ed686ac3c..f73bf17dfe 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 7533fa5f6c..2f3d4df329 100644
--- a/scene/register_scene_types.cpp
+++ b/scene/register_scene_types.cpp
@@ -404,6 +404,7 @@ void register_scene_types() {
 	ClassDB::register_class<Curve3D>();
 	ClassDB::register_class<Path>();
 	ClassDB::register_class<PathFollow>();
+	ClassDB::register_class<OrientedPathFollow>();
 	ClassDB::register_class<VisibilityNotifier>();
 	ClassDB::register_class<VisibilityEnabler>();
 	ClassDB::register_class<WorldEnvironment>();
diff --git a/scene/resources/curve.cpp b/scene/resources/curve.cpp
index 4ec1e8973d..7f902fc982 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<Plane>::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 492eb05d1e..9cb12a4345 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<float, Vector3> &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;