draw fish bones for Path2D and Path3D

These fish bones are add to indicate the direction and local transforms alone the path.

2 files changed, 49 insertions, 11 deletions

diff --git a/scene/2d/path_2d.cpp b/scene/2d/path_2d.cpp
index b5945a4562..b990e47666 100644
--- a/scene/2d/path_2d.cpp
+++ b/scene/2d/path_2d.cpp
@@ -106,18 +106,57 @@ void Path2D::_notification(int p_what) {
 #else
 			const real_t line_width = get_tree()->get_debug_paths_width();
 #endif
-			_cached_draw_pts.resize(curve->get_point_count() * 8);
-			int count = 0;
-
-			for (int i = 0; i < curve->get_point_count(); i++) {
-				for (int j = 0; j < 8; j++) {
-					real_t frac = j * (1.0 / 8.0);
-					Vector2 p = curve->sample(i, frac);
-					_cached_draw_pts.set(count++, p);
+			real_t interval = 10;
+			const real_t length = curve->get_baked_length();
+
+			if (length > CMP_EPSILON) {
+				const int sample_count = int(length / interval) + 2;
+				interval = length / (sample_count - 1); // Recalculate real interval length.
+
+				Vector<Transform2D> frames;
+				frames.resize(sample_count);
+
+				{
+					Transform2D *w = frames.ptrw();
+
+					for (int i = 0; i < sample_count; i++) {
+						w[i] = curve->sample_baked_with_rotation(i * interval, true, true);
+					}
 				}
-			}
 
-			draw_polyline(_cached_draw_pts, get_tree()->get_debug_paths_color(), line_width, true);
+				const Transform2D *r = frames.ptr();
+				// Draw curve segments
+				{
+					PackedVector2Array v2p;
+					v2p.resize(sample_count);
+					Vector2 *w = v2p.ptrw();
+
+					for (int i = 0; i < sample_count; i++) {
+						w[i] = r[i].get_origin();
+					}
+					draw_polyline(v2p, get_tree()->get_debug_paths_color(), line_width, false);
+				}
+
+				// Draw fish bones
+				{
+					PackedVector2Array v2p;
+					v2p.resize(3);
+					Vector2 *w = v2p.ptrw();
+
+					for (int i = 0; i < sample_count; i++) {
+						const Vector2 p = r[i].get_origin();
+						const Vector2 side = r[i].columns[0];
+						const Vector2 forward = r[i].columns[1];
+
+						// Fish Bone.
+						w[0] = p + (side - forward) * 5;
+						w[1] = p;
+						w[2] = p + (-side - forward) * 5;
+
+						draw_polyline(v2p, get_tree()->get_debug_paths_color(), line_width * 0.5, false);
+					}
+				}
+			}
 		} break;
 	}
 }
diff --git a/scene/2d/path_2d.h b/scene/2d/path_2d.h
index 5e436fb9f6..935717605a 100644
--- a/scene/2d/path_2d.h
+++ b/scene/2d/path_2d.h
@@ -38,7 +38,6 @@ class Path2D : public Node2D {
 	GDCLASS(Path2D, Node2D);
 
 	Ref<Curve2D> curve;
-	Vector<Vector2> _cached_draw_pts;
 
 	void _curve_changed();