From 91f365b183b3bfafbb40157c82b6c2e55b10f104 Mon Sep 17 00:00:00 2001
From: Relintai <relintai@protonmail.com>
Date: Wed, 7 Jun 2023 00:36:53 +0200
Subject: [PATCH] Added CapsuleShape2D support for
 StaticBody2DNavigationGeometryParser2D.

---
 ...ticbody2d_navigation_geometry_parser_2d.cpp | 18 +++++++++++++++---
 scene/resources/capsule_shape_2d.cpp           |  6 +++---
 scene/resources/capsule_shape_2d.h             |  3 ++-
 3 files changed, 20 insertions(+), 7 deletions(-)

diff --git a/modules/navigation_geometry_parsers/geometry_parser_2d/staticbody2d_navigation_geometry_parser_2d.cpp b/modules/navigation_geometry_parsers/geometry_parser_2d/staticbody2d_navigation_geometry_parser_2d.cpp
index 70b712c56..400357ae0 100644
--- a/modules/navigation_geometry_parsers/geometry_parser_2d/staticbody2d_navigation_geometry_parser_2d.cpp
+++ b/modules/navigation_geometry_parsers/geometry_parser_2d/staticbody2d_navigation_geometry_parser_2d.cpp
@@ -92,10 +92,22 @@ void StaticBody2DNavigationGeometryParser2D::parse_geometry(Node *p_node, Ref<Na
 
 					CapsuleShape2D *capsule_shape = Object::cast_to<CapsuleShape2D>(*s);
 					if (capsule_shape) {
-						//const real_t capsule_height = capsule_shape->get_height();
-						//const real_t capsule_radius = capsule_shape->get_radius();
+						PoolVector<Vector2> shape_outline;
 
-						//p_source_geometry->add_mesh_array(arr, transform);
+						Vector<Vector2> points = capsule_shape->get_points();
+
+						shape_outline.resize(points.size() + 1);
+						PoolVector<Vector2>::Write shape_outline_write = shape_outline.write();
+
+						for (int i = 0; i < points.size(); ++i) {
+							shape_outline_write[i] = transform.xform(points[i]);
+						}
+
+						if (points.size() > 0) {
+							shape_outline_write[points.size()] = transform.xform(points[0]);
+						}
+
+						p_source_geometry->add_obstruction_outline(shape_outline);
 					}
 
 					CircleShape2D *circle_shape = Object::cast_to<CircleShape2D>(*s);
diff --git a/scene/resources/capsule_shape_2d.cpp b/scene/resources/capsule_shape_2d.cpp
index 20b4ee476..51216a34b 100644
--- a/scene/resources/capsule_shape_2d.cpp
+++ b/scene/resources/capsule_shape_2d.cpp
@@ -33,7 +33,7 @@
 #include "servers/physics_2d_server.h"
 #include "servers/rendering_server.h"
 
-Vector<Vector2> CapsuleShape2D::_get_points() const {
+Vector<Vector2> CapsuleShape2D::get_points() const {
 	Vector<Vector2> points;
 	for (int i = 0; i < 24; i++) {
 		Vector2 ofs = Vector2(0, (i > 6 && i <= 18) ? -get_height() * 0.5 : get_height() * 0.5);
@@ -48,7 +48,7 @@ Vector<Vector2> CapsuleShape2D::_get_points() const {
 }
 
 bool CapsuleShape2D::_edit_is_selected_on_click(const Point2 &p_point, double p_tolerance) const {
-	return Geometry::is_point_in_polygon(p_point, _get_points());
+	return Geometry::is_point_in_polygon(p_point, get_points());
 }
 
 void CapsuleShape2D::_update_shape() {
@@ -79,7 +79,7 @@ real_t CapsuleShape2D::get_height() const {
 }
 
 void CapsuleShape2D::draw(const RID &p_to_rid, const Color &p_color) {
-	Vector<Vector2> points = _get_points();
+	Vector<Vector2> points = get_points();
 	Vector<Color> col;
 	col.push_back(p_color);
 	RenderingServer::get_singleton()->canvas_item_add_polygon(p_to_rid, points, col);
diff --git a/scene/resources/capsule_shape_2d.h b/scene/resources/capsule_shape_2d.h
index 004a89eae..27dd4e898 100644
--- a/scene/resources/capsule_shape_2d.h
+++ b/scene/resources/capsule_shape_2d.h
@@ -39,7 +39,6 @@ class CapsuleShape2D : public Shape2D {
 	real_t radius;
 
 	void _update_shape();
-	Vector<Vector2> _get_points() const;
 
 protected:
 	static void _bind_methods();
@@ -57,6 +56,8 @@ public:
 	virtual Rect2 get_rect() const;
 	virtual real_t get_enclosing_radius() const;
 
+	Vector<Vector2> get_points() const;
+
 	CapsuleShape2D();
 };