Up vector implementation and OrientedPathFollow.

1 files changed, 216 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;
+}