From f12f5b36b58e3269cfa981744e4702ef68e81b49 Mon Sep 17 00:00:00 2001 From: Tomasz Chabora Date: Sun, 30 Aug 2020 23:15:00 +0200 Subject: [PATCH] Make radius & height in CapsuleShape2D independent --- doc/classes/CapsuleShape2D.xml | 2 +- .../collision_shape_2d_editor_plugin.cpp | 6 +-- scene/resources/capsule_shape_2d.cpp | 21 +++++---- scene/resources/capsule_shape_2d.h | 2 +- .../physics_2d/collision_solver_2d_sat.cpp | 43 +++++++++++++------ servers/physics_2d/shape_2d_sw.cpp | 18 ++++---- servers/physics_2d/shape_2d_sw.h | 4 +- 7 files changed, 56 insertions(+), 40 deletions(-) diff --git a/doc/classes/CapsuleShape2D.xml b/doc/classes/CapsuleShape2D.xml index 290d260f0f..8ed7d56557 100644 --- a/doc/classes/CapsuleShape2D.xml +++ b/doc/classes/CapsuleShape2D.xml @@ -11,7 +11,7 @@ - + The capsule's height. diff --git a/editor/plugins/collision_shape_2d_editor_plugin.cpp b/editor/plugins/collision_shape_2d_editor_plugin.cpp index 5a6b7d98d3..4266e0f676 100644 --- a/editor/plugins/collision_shape_2d_editor_plugin.cpp +++ b/editor/plugins/collision_shape_2d_editor_plugin.cpp @@ -120,7 +120,7 @@ void CollisionShape2DEditor::set_handle(int idx, Point2 &p_point) { if (idx == 0) { capsule->set_radius(parameter); } else if (idx == 1) { - capsule->set_height(parameter * 2 - capsule->get_radius() * 2); + capsule->set_height(parameter * 2); } canvas_item_editor->update_viewport(); @@ -440,8 +440,8 @@ void CollisionShape2DEditor::forward_canvas_draw_over_viewport(Control *p_overla float radius = shape->get_radius(); float height = shape->get_height() / 2; - handles.write[0] = Point2(radius, height); - handles.write[1] = Point2(0, height + radius); + handles.write[0] = Point2(radius, 0); + handles.write[1] = Point2(0, height); p_overlay->draw_texture(h, gt.xform(handles[0]) - size); p_overlay->draw_texture(h, gt.xform(handles[1]) - size); diff --git a/scene/resources/capsule_shape_2d.cpp b/scene/resources/capsule_shape_2d.cpp index e5edba8a67..8f7f3edc28 100644 --- a/scene/resources/capsule_shape_2d.cpp +++ b/scene/resources/capsule_shape_2d.cpp @@ -38,11 +38,11 @@ Vector CapsuleShape2D::_get_points() const { Vector points; const real_t turn_step = Math_TAU / 24.0; for (int i = 0; i < 24; i++) { - Vector2 ofs = Vector2(0, (i > 6 && i <= 18) ? -get_height() * 0.5 : get_height() * 0.5); + Vector2 ofs = Vector2(0, (i > 6 && i <= 18) ? -height * 0.5 + radius : height * 0.5 - radius); - points.push_back(Vector2(Math::sin(i * turn_step), Math::cos(i * turn_step)) * get_radius() + ofs); + points.push_back(Vector2(Math::sin(i * turn_step), Math::cos(i * turn_step)) * radius + ofs); if (i == 6 || i == 18) { - points.push_back(Vector2(Math::sin(i * turn_step), Math::cos(i * turn_step)) * get_radius() - ofs); + points.push_back(Vector2(Math::sin(i * turn_step), Math::cos(i * turn_step)) * radius - ofs); } } @@ -60,6 +60,9 @@ void CapsuleShape2D::_update_shape() { void CapsuleShape2D::set_radius(real_t p_radius) { radius = p_radius; + if (radius > height * 0.5) { + height = radius * 2; + } _update_shape(); } @@ -69,8 +72,8 @@ real_t CapsuleShape2D::get_radius() const { void CapsuleShape2D::set_height(real_t p_height) { height = p_height; - if (height < 0) { - height = 0; + if (radius > height * 0.5) { + height = radius * 2; } _update_shape(); @@ -93,15 +96,11 @@ void CapsuleShape2D::draw(const RID &p_to_rid, const Color &p_color) { } Rect2 CapsuleShape2D::get_rect() const { - Vector2 he = Point2(get_radius(), get_radius() + get_height() * 0.5); - Rect2 rect; - rect.position = -he; - rect.size = he * 2.0; - return rect; + return Rect2(0, 0, radius, height); } real_t CapsuleShape2D::get_enclosing_radius() const { - return radius + height * 0.5; + return height * 0.5; } void CapsuleShape2D::_bind_methods() { diff --git a/scene/resources/capsule_shape_2d.h b/scene/resources/capsule_shape_2d.h index 439b67e8c3..37b6c52c0e 100644 --- a/scene/resources/capsule_shape_2d.h +++ b/scene/resources/capsule_shape_2d.h @@ -36,7 +36,7 @@ class CapsuleShape2D : public Shape2D { GDCLASS(CapsuleShape2D, Shape2D); - real_t height = 20.0; + real_t height = 30.0; real_t radius = 10.0; void _update_shape(); 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));