Make radius & height in CapsuleShape2D independent

3 files changed, 41 insertions, 24 deletions

diff --git a/servers/physics_2d/collision_solver_2d_sat.cpp b/servers/physics_2d/collision_solver_2d_sat.cpp
index 5b75d1044a..45b9e6414d 100644
--- a/servers/physics_2d/collision_solver_2d_sat.cpp
+++ b/servers/physics_2d/collision_solver_2d_sat.cpp
@@ -560,16 +560,18 @@ static void _collision_segment_capsule(const Shape2DSW *p_a, const Transform2D &
 		return;
 	}
 
-	if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * 0.5))) {
+	real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius();
+
+	if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir))) {
 		return;
 	}
-	if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * -0.5))) {
+	if (TEST_POINT(p_transform_a.xform(segment_A->get_a()), (p_transform_b.get_origin() - p_transform_b.elements[1] * capsule_dir))) {
 		return;
 	}
-	if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * 0.5))) {
+	if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir))) {
 		return;
 	}
-	if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * -0.5))) {
+	if (TEST_POINT(p_transform_a.xform(segment_A->get_b()), (p_transform_b.get_origin() - p_transform_b.elements[1] * capsule_dir))) {
 		return;
 	}
 
@@ -715,11 +717,13 @@ static void _collision_circle_capsule(const Shape2DSW *p_a, const Transform2D &p
 		return;
 	}
 
+	real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius();
+
 	//capsule endpoints
-	if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * 0.5))) {
+	if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir))) {
 		return;
 	}
-	if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * -0.5))) {
+	if (TEST_POINT(p_transform_a.get_origin(), (p_transform_b.get_origin() - p_transform_b.elements[1] * capsule_dir))) {
 		return;
 	}
 
@@ -864,9 +868,11 @@ static void _collision_rectangle_capsule(const Shape2DSW *p_a, const Transform2D
 
 	Transform2D boxinv = p_transform_a.affine_inverse();
 
+	real_t capsule_dir = capsule_B->get_height() * 0.5 - capsule_B->get_radius();
+
 	for (int i = 0; i < 2; i++) {
 		{
-			Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (i == 0 ? 0.5 : -0.5);
+			Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir;
 
 			if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) {
 				return;
@@ -874,7 +880,7 @@ static void _collision_rectangle_capsule(const Shape2DSW *p_a, const Transform2D
 		}
 
 		if (castA) {
-			Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (i == 0 ? 0.5 : -0.5);
+			Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir;
 			capsule_endpoint -= p_motion_a;
 
 			if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) {
@@ -883,7 +889,7 @@ static void _collision_rectangle_capsule(const Shape2DSW *p_a, const Transform2D
 		}
 
 		if (castB) {
-			Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (i == 0 ? 0.5 : -0.5);
+			Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir;
 			capsule_endpoint += p_motion_b;
 
 			if (!separator.test_axis(rectangle_A->get_circle_axis(p_transform_a, boxinv, capsule_endpoint))) {
@@ -892,7 +898,7 @@ static void _collision_rectangle_capsule(const Shape2DSW *p_a, const Transform2D
 		}
 
 		if (castA && castB) {
-			Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (i == 0 ? 0.5 : -0.5);
+			Vector2 capsule_endpoint = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir;
 			capsule_endpoint -= p_motion_a;
 			capsule_endpoint += p_motion_b;
 
@@ -900,6 +906,8 @@ static void _collision_rectangle_capsule(const Shape2DSW *p_a, const Transform2D
 				return;
 			}
 		}
+
+		capsule_dir *= -1.0;
 	}
 
 	separator.generate_contacts();
@@ -994,16 +1002,22 @@ static void _collision_capsule_capsule(const Shape2DSW *p_a, const Transform2D &
 
 	//capsule endpoints
 
+	real_t capsule_dir_A = capsule_A->get_height() * 0.5 - capsule_A->get_radius();
 	for (int i = 0; i < 2; i++) {
-		Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.elements[1] * capsule_A->get_height() * (i == 0 ? 0.5 : -0.5);
+		Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.elements[1] * capsule_dir_A;
 
+		real_t capsule_dir_B = capsule_B->get_height() * 0.5 - capsule_B->get_radius();
 		for (int j = 0; j < 2; j++) {
-			Vector2 capsule_endpoint_B = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_B->get_height() * (j == 0 ? 0.5 : -0.5);
+			Vector2 capsule_endpoint_B = p_transform_b.get_origin() + p_transform_b.elements[1] * capsule_dir_B;
 
 			if (TEST_POINT(capsule_endpoint_A, capsule_endpoint_B)) {
 				return;
 			}
+
+			capsule_dir_B *= -1.0;
 		}
+
+		capsule_dir_A *= -1.0;
 	}
 
 	separator.generate_contacts();
@@ -1034,12 +1048,15 @@ static void _collision_capsule_convex_polygon(const Shape2DSW *p_a, const Transf
 	for (int i = 0; i < convex_B->get_point_count(); i++) {
 		Vector2 cpoint = p_transform_b.xform(convex_B->get_point(i));
 
+		real_t capsule_dir = capsule_A->get_height() * 0.5 - capsule_A->get_radius();
 		for (int j = 0; j < 2; j++) {
-			Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.elements[1] * capsule_A->get_height() * (j == 0 ? 0.5 : -0.5);
+			Vector2 capsule_endpoint_A = p_transform_a.get_origin() + p_transform_a.elements[1] * capsule_dir;
 
 			if (TEST_POINT(capsule_endpoint_A, cpoint)) {
 				return;
 			}
+
+			capsule_dir *= -1.0;
 		}
 
 		if (!separator.test_axis(convex_B->get_xformed_segment_normal(p_transform_b, i))) {
diff --git a/servers/physics_2d/shape_2d_sw.cpp b/servers/physics_2d/shape_2d_sw.cpp
index ee0923effd..b16efbfe64 100644
--- a/servers/physics_2d/shape_2d_sw.cpp
+++ b/servers/physics_2d/shape_2d_sw.cpp
@@ -343,15 +343,15 @@ void CapsuleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports
 
 		r_amount = 2;
 		r_supports[0] = n;
-		r_supports[0].y += height * 0.5;
+		r_supports[0].y += height * 0.5 - radius;
 		r_supports[1] = n;
-		r_supports[1].y -= height * 0.5;
+		r_supports[1].y -= height * 0.5 - radius;
 
 	} else {
-		real_t h = (d > 0) ? height : -height;
+		real_t h = height * 0.5 - radius;
 
 		n *= radius;
-		n.y += h * 0.5;
+		n.y += (d > 0) ? h : -h;
 		r_amount = 1;
 		*r_supports = n;
 	}
@@ -360,7 +360,7 @@ void CapsuleShape2DSW::get_supports(const Vector2 &p_normal, Vector2 *r_supports
 bool CapsuleShape2DSW::contains_point(const Vector2 &p_point) const {
 	Vector2 p = p_point;
 	p.y = Math::abs(p.y);
-	p.y -= height * 0.5;
+	p.y -= height * 0.5 - radius;
 	if (p.y < 0) {
 		p.y = 0;
 	}
@@ -377,7 +377,7 @@ bool CapsuleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &
 	for (int i = 0; i < 2; i++) {
 		Vector2 begin = p_begin;
 		Vector2 end = p_end;
-		real_t ofs = (i == 0) ? -height * 0.5 : height * 0.5;
+		real_t ofs = (i == 0) ? -height * 0.5 + radius : height * 0.5 - radius;
 		begin.y += ofs;
 		end.y += ofs;
 
@@ -414,7 +414,7 @@ bool CapsuleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &
 	}
 
 	Vector2 rpos, rnorm;
-	if (Rect2(Point2(-radius, -height * 0.5), Size2(radius * 2.0, height)).intersects_segment(p_begin, p_end, &rpos, &rnorm)) {
+	if (Rect2(Point2(-radius, -height * 0.5 + radius), Size2(radius * 2.0, height - radius * 2)).intersects_segment(p_begin, p_end, &rpos, &rnorm)) {
 		real_t pd = n.dot(rpos);
 		if (pd < d) {
 			r_point = rpos;
@@ -429,7 +429,7 @@ bool CapsuleShape2DSW::intersect_segment(const Vector2 &p_begin, const Vector2 &
 }
 
 real_t CapsuleShape2DSW::get_moment_of_inertia(real_t p_mass, const Size2 &p_scale) const {
-	Vector2 he2 = Vector2(radius * 2, height + radius * 2) * p_scale;
+	Vector2 he2 = Vector2(radius * 2, height) * p_scale;
 	return p_mass * he2.dot(he2) / 12.0;
 }
 
@@ -447,7 +447,7 @@ void CapsuleShape2DSW::set_data(const Variant &p_data) {
 		height = p.y;
 	}
 
-	Point2 he(radius, height * 0.5 + radius);
+	Point2 he(radius, height * 0.5);
 	configure(Rect2(-he, he * 2));
 }
 
diff --git a/servers/physics_2d/shape_2d_sw.h b/servers/physics_2d/shape_2d_sw.h
index be2eeb8ed3..3e4271b156 100644
--- a/servers/physics_2d/shape_2d_sw.h
+++ b/servers/physics_2d/shape_2d_sw.h
@@ -361,10 +361,10 @@ public:
 	_FORCE_INLINE_ void project_range(const Vector2 &p_normal, const Transform2D &p_transform, real_t &r_min, real_t &r_max) const {
 		// no matter the angle, the box is mirrored anyway
 		Vector2 n = p_transform.basis_xform_inv(p_normal).normalized();
-		real_t h = (n.y > 0) ? height : -height;
+		real_t h = height * 0.5 - radius;
 
 		n *= radius;
-		n.y += h * 0.5;
+		n.y += (n.y > 0) ? h : -h;
 
 		r_max = p_normal.dot(p_transform.xform(n));
 		r_min = p_normal.dot(p_transform.xform(-n));