1 files changed, 101 insertions, 6 deletions

diff --git a/scene/3d/path_3d.cpp b/scene/3d/path_3d.cpp
index 8c2b1c6889..25226ad384 100644
--- a/scene/3d/path_3d.cpp
+++ b/scene/3d/path_3d.cpp
@@ -30,6 +30,92 @@
 
 #include "path_3d.h"
 
+Path3D::Path3D() {
+	SceneTree *st = SceneTree::get_singleton();
+	if (st && st->is_debugging_paths_hint()) {
+		debug_instance = RS::get_singleton()->instance_create();
+		set_notify_transform(true);
+		_update_debug_mesh();
+	}
+}
+
+Path3D::~Path3D() {
+	if (debug_instance.is_valid()) {
+		RS::get_singleton()->free(debug_instance);
+	}
+	if (debug_mesh.is_valid()) {
+		RS::get_singleton()->free(debug_mesh->get_rid());
+	}
+}
+
+void Path3D::_notification(int p_what) {
+	switch (p_what) {
+		case NOTIFICATION_ENTER_TREE: {
+			SceneTree *st = SceneTree::get_singleton();
+			if (st && st->is_debugging_paths_hint()) {
+				_update_debug_mesh();
+			}
+		} break;
+
+		case NOTIFICATION_EXIT_TREE: {
+			SceneTree *st = SceneTree::get_singleton();
+			if (st && st->is_debugging_paths_hint()) {
+				RS::get_singleton()->instance_set_visible(debug_instance, false);
+			}
+		} break;
+
+		case NOTIFICATION_TRANSFORM_CHANGED: {
+			if (is_inside_tree() && debug_instance.is_valid()) {
+				RS::get_singleton()->instance_set_transform(debug_instance, get_global_transform());
+			}
+		} break;
+	}
+}
+
+void Path3D::_update_debug_mesh() {
+	SceneTree *st = SceneTree::get_singleton();
+	if (!(st && st->is_debugging_paths_hint())) {
+		return;
+	}
+
+	if (!debug_mesh.is_valid()) {
+		debug_mesh = Ref<ArrayMesh>(memnew(ArrayMesh));
+	}
+
+	if (!(curve.is_valid())) {
+		RS::get_singleton()->instance_set_visible(debug_instance, false);
+		return;
+	}
+	if (curve->get_point_count() < 2) {
+		RS::get_singleton()->instance_set_visible(debug_instance, false);
+		return;
+	}
+
+	Vector<Vector3> vertex_array;
+
+	for (int i = 1; i < curve->get_point_count(); i++) {
+		Vector3 line_end = curve->get_point_position(i);
+		Vector3 line_start = curve->get_point_position(i - 1);
+		vertex_array.push_back(line_start);
+		vertex_array.push_back(line_end);
+	}
+
+	Array mesh_array;
+	mesh_array.resize(Mesh::ARRAY_MAX);
+	mesh_array[Mesh::ARRAY_VERTEX] = vertex_array;
+
+	debug_mesh->clear_surfaces();
+	debug_mesh->add_surface_from_arrays(Mesh::PRIMITIVE_LINES, mesh_array);
+
+	RS::get_singleton()->instance_set_base(debug_instance, debug_mesh->get_rid());
+	RS::get_singleton()->mesh_surface_set_material(debug_mesh->get_rid(), 0, st->get_debug_paths_material()->get_rid());
+	if (is_inside_tree()) {
+		RS::get_singleton()->instance_set_scenario(debug_instance, get_world_3d()->get_scenario());
+		RS::get_singleton()->instance_set_transform(debug_instance, get_global_transform());
+		RS::get_singleton()->instance_set_visible(debug_instance, is_visible_in_tree());
+	}
+}
+
 void Path3D::_curve_changed() {
 	if (is_inside_tree() && Engine::get_singleton()->is_editor_hint()) {
 		update_gizmos();
@@ -48,6 +134,10 @@ void Path3D::_curve_changed() {
 			}
 		}
 	}
+	SceneTree *st = SceneTree::get_singleton();
+	if (st && st->is_debugging_paths_hint()) {
+		_update_debug_mesh();
+	}
 }
 
 void Path3D::set_curve(const Ref<Curve3D> &p_curve) {
@@ -157,10 +247,14 @@ void PathFollow3D::_update_transform(bool p_update_xyz_rot) {
 		// for a discussion about why not Frenet frame.
 
 		t.origin = pos;
-
-		if (p_update_xyz_rot && delta_offset != 0) { // Only update rotation if some parameter has changed - i.e. not on addition to scene tree.
-			Vector3 t_prev = (pos - c->interpolate_baked(offset - delta_offset, cubic)).normalized();
-			Vector3 t_cur = (c->interpolate_baked(offset + delta_offset, cubic) - pos).normalized();
+		if (p_update_xyz_rot && prev_offset != offset) { // 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->interpolate_baked(prev_offset - sample_distance, cubic);
+			Vector3 t_prev_pos_b = c->interpolate_baked(prev_offset + sample_distance, cubic);
+			Vector3 t_cur_pos_a = c->interpolate_baked(offset - sample_distance, cubic);
+			Vector3 t_cur_pos_b = c->interpolate_baked(offset + 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);
@@ -202,7 +296,7 @@ void PathFollow3D::_update_transform(bool p_update_xyz_rot) {
 			}
 		}
 
-		t.translate(Vector3(h_offset, v_offset, 0));
+		t.translate_local(Vector3(h_offset, v_offset, 0));
 	} else {
 		t.origin = pos + Vector3(h_offset, v_offset, 0);
 	}
@@ -303,7 +397,8 @@ void PathFollow3D::_bind_methods() {
 }
 
 void PathFollow3D::set_offset(real_t p_offset) {
-	delta_offset = p_offset - offset;
+	ERR_FAIL_COND(!isfinite(p_offset));
+	prev_offset = offset;
 	offset = p_offset;
 
 	if (path) {