From f9176a39ce48685adacca2698b8da8136c5de166 Mon Sep 17 00:00:00 2001 From: PouleyKetchoupp Date: Wed, 11 Aug 2021 10:38:14 -0700 Subject: [PATCH] Make radius & height in CapsuleShape3D independent Also changed CapsuleMesh to make settings consistent between render and physics. --- doc/classes/CapsuleMesh.xml | 5 ++- doc/classes/CapsuleShape3D.xml | 2 +- editor/plugins/node_3d_editor_gizmos.cpp | 9 ++---- editor/plugins/skeleton_3d_editor_plugin.cpp | 2 +- .../navigation/navigation_mesh_generator.cpp | 2 +- scene/resources/capsule_shape_3d.cpp | 10 ++++-- scene/resources/capsule_shape_3d.h | 2 +- scene/resources/primitive_meshes.cpp | 28 +++++++++------- scene/resources/primitive_meshes.h | 6 ++-- .../physics_3d/collision_solver_3d_sat.cpp | 16 +++++----- servers/physics_3d/shape_3d_sw.cpp | 32 +++++++++---------- servers/physics_3d/shape_3d_sw.h | 2 +- 12 files changed, 62 insertions(+), 54 deletions(-) diff --git a/doc/classes/CapsuleMesh.xml b/doc/classes/CapsuleMesh.xml index 031abd01121..b8605ccaecf 100644 --- a/doc/classes/CapsuleMesh.xml +++ b/doc/classes/CapsuleMesh.xml @@ -11,9 +11,8 @@ - - Height of the middle cylindrical part of the capsule (without the hemispherical ends). - [b]Note:[/b] The capsule's total height is equal to [member mid_height] + 2 * [member radius]. + + Total height of the capsule mesh (including the hemispherical ends). Number of radial segments on the capsule mesh. diff --git a/doc/classes/CapsuleShape3D.xml b/doc/classes/CapsuleShape3D.xml index 27a6242bc9b..c2b13224cf5 100644 --- a/doc/classes/CapsuleShape3D.xml +++ b/doc/classes/CapsuleShape3D.xml @@ -12,7 +12,7 @@ - + The capsule's height. diff --git a/editor/plugins/node_3d_editor_gizmos.cpp b/editor/plugins/node_3d_editor_gizmos.cpp index 8b098ad23b1..dcea7b26f32 100644 --- a/editor/plugins/node_3d_editor_gizmos.cpp +++ b/editor/plugins/node_3d_editor_gizmos.cpp @@ -4168,14 +4168,11 @@ void CollisionShape3DGizmoPlugin::set_handle(const EditorNode3DGizmo *p_gizmo, i if (Object::cast_to(*s)) { Vector3 axis; - axis[p_id == 0 ? 0 : 2] = 1.0; + axis[p_id == 0 ? 0 : 1] = 1.0; Ref cs2 = s; Vector3 ra, rb; Geometry3D::get_closest_points_between_segments(Vector3(), axis * 4096, sg[0], sg[1], ra, rb); float d = axis.dot(ra); - if (p_id == 1) { - d -= cs2->get_radius(); - } if (Node3DEditor::get_singleton()->is_snap_enabled()) { d = Math::snapped(d, Node3DEditor::get_singleton()->get_translate_snap()); @@ -4397,7 +4394,7 @@ void CollisionShape3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) { Vector points; - Vector3 d(0, height * 0.5, 0); + Vector3 d(0, height * 0.5 - radius, 0); for (int i = 0; i < 360; i++) { float ra = Math::deg2rad((float)i); float rb = Math::deg2rad((float)i + 1); @@ -4456,7 +4453,7 @@ void CollisionShape3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) { Vector handles; handles.push_back(Vector3(cs2->get_radius(), 0, 0)); - handles.push_back(Vector3(0, cs2->get_height() * 0.5 + cs2->get_radius(), 0)); + handles.push_back(Vector3(0, cs2->get_height() * 0.5, 0)); p_gizmo->add_handles(handles, handles_material); } diff --git a/editor/plugins/skeleton_3d_editor_plugin.cpp b/editor/plugins/skeleton_3d_editor_plugin.cpp index 3de2ffc137e..0bb0bfde6f6 100644 --- a/editor/plugins/skeleton_3d_editor_plugin.cpp +++ b/editor/plugins/skeleton_3d_editor_plugin.cpp @@ -386,7 +386,7 @@ PhysicalBone3D *Skeleton3DEditor::create_physical_bone(int bone_id, int bone_chi const real_t radius(half_height * 0.2); CapsuleShape3D *bone_shape_capsule = memnew(CapsuleShape3D); - bone_shape_capsule->set_height((half_height - radius) * 2); + bone_shape_capsule->set_height(half_height * 2); bone_shape_capsule->set_radius(radius); CollisionShape3D *bone_shape = memnew(CollisionShape3D); diff --git a/modules/navigation/navigation_mesh_generator.cpp b/modules/navigation/navigation_mesh_generator.cpp index 74da939e6fd..905d10c9d42 100644 --- a/modules/navigation/navigation_mesh_generator.cpp +++ b/modules/navigation/navigation_mesh_generator.cpp @@ -187,7 +187,7 @@ void NavigationMeshGenerator::_parse_geometry(Transform3D p_accumulated_transfor Ref capsule_mesh; capsule_mesh.instantiate(); capsule_mesh->set_radius(capsule->get_radius()); - capsule_mesh->set_mid_height(capsule->get_height() / 2.0); + capsule_mesh->set_height(capsule->get_height()); mesh = capsule_mesh; } diff --git a/scene/resources/capsule_shape_3d.cpp b/scene/resources/capsule_shape_3d.cpp index 226fe1ecd2d..e267941d6aa 100644 --- a/scene/resources/capsule_shape_3d.cpp +++ b/scene/resources/capsule_shape_3d.cpp @@ -37,7 +37,7 @@ Vector CapsuleShape3D::get_debug_mesh_lines() const { Vector points; - Vector3 d(0, height * 0.5, 0); + Vector3 d(0, height * 0.5 - radius, 0); for (int i = 0; i < 360; i++) { float ra = Math::deg2rad((float)i); float rb = Math::deg2rad((float)i + 1); @@ -67,7 +67,7 @@ Vector CapsuleShape3D::get_debug_mesh_lines() const { } real_t CapsuleShape3D::get_enclosing_radius() const { - return radius + height * 0.5; + return height * 0.5; } void CapsuleShape3D::_update_shape() { @@ -80,6 +80,9 @@ void CapsuleShape3D::_update_shape() { void CapsuleShape3D::set_radius(float p_radius) { radius = p_radius; + if (radius > height * 0.5) { + radius = height * 0.5; + } _update_shape(); notify_change_to_owners(); } @@ -90,6 +93,9 @@ float CapsuleShape3D::get_radius() const { void CapsuleShape3D::set_height(float p_height) { height = p_height; + if (radius > height * 0.5) { + height = radius * 2; + } _update_shape(); notify_change_to_owners(); } diff --git a/scene/resources/capsule_shape_3d.h b/scene/resources/capsule_shape_3d.h index 25645ecf9df..f09b4fb77d8 100644 --- a/scene/resources/capsule_shape_3d.h +++ b/scene/resources/capsule_shape_3d.h @@ -36,7 +36,7 @@ class CapsuleShape3D : public Shape3D { GDCLASS(CapsuleShape3D, Shape3D); float radius = 1.0; - float height = 1.0; + float height = 3.0; protected: static void _bind_methods(); diff --git a/scene/resources/primitive_meshes.cpp b/scene/resources/primitive_meshes.cpp index dfa45cc8108..ba85ea4a6ca 100644 --- a/scene/resources/primitive_meshes.cpp +++ b/scene/resources/primitive_meshes.cpp @@ -300,7 +300,7 @@ void CapsuleMesh::_create_mesh_array(Array &p_arr) const { z = cos(u * Math_TAU); Vector3 p = Vector3(x * radius * w, y, -z * radius * w); - points.push_back(p + Vector3(0.0, 0.5 * mid_height, 0.0)); + points.push_back(p + Vector3(0.0, 0.5 * height - radius, 0.0)); normals.push_back(p.normalized()); ADD_TANGENT(z, 0.0, x, 1.0) uvs.push_back(Vector2(u, v * onethird)); @@ -328,8 +328,8 @@ void CapsuleMesh::_create_mesh_array(Array &p_arr) const { v = j; v /= (rings + 1); - y = mid_height * v; - y = (mid_height * 0.5) - y; + y = (height - 2.0 * radius) * v; + y = (0.5 * height - radius) - y; for (i = 0; i <= radial_segments; i++) { u = i; @@ -379,7 +379,7 @@ void CapsuleMesh::_create_mesh_array(Array &p_arr) const { z = cos(u2 * Math_TAU); Vector3 p = Vector3(x * radius * w, y, -z * radius * w); - points.push_back(p + Vector3(0.0, -0.5 * mid_height, 0.0)); + points.push_back(p + Vector3(0.0, -0.5 * height + radius, 0.0)); normals.push_back(p.normalized()); ADD_TANGENT(z, 0.0, x, 1.0) uvs.push_back(Vector2(u2, twothirds + ((v - 1.0) * onethird))); @@ -410,8 +410,8 @@ void CapsuleMesh::_create_mesh_array(Array &p_arr) const { void CapsuleMesh::_bind_methods() { ClassDB::bind_method(D_METHOD("set_radius", "radius"), &CapsuleMesh::set_radius); ClassDB::bind_method(D_METHOD("get_radius"), &CapsuleMesh::get_radius); - ClassDB::bind_method(D_METHOD("set_mid_height", "mid_height"), &CapsuleMesh::set_mid_height); - ClassDB::bind_method(D_METHOD("get_mid_height"), &CapsuleMesh::get_mid_height); + ClassDB::bind_method(D_METHOD("set_height", "height"), &CapsuleMesh::set_height); + ClassDB::bind_method(D_METHOD("get_height"), &CapsuleMesh::get_height); ClassDB::bind_method(D_METHOD("set_radial_segments", "segments"), &CapsuleMesh::set_radial_segments); ClassDB::bind_method(D_METHOD("get_radial_segments"), &CapsuleMesh::get_radial_segments); @@ -419,13 +419,16 @@ void CapsuleMesh::_bind_methods() { ClassDB::bind_method(D_METHOD("get_rings"), &CapsuleMesh::get_rings); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "radius", PROPERTY_HINT_RANGE, "0.001,100.0,0.001,or_greater"), "set_radius", "get_radius"); - ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "mid_height", PROPERTY_HINT_RANGE, "0.001,100.0,0.001,or_greater"), "set_mid_height", "get_mid_height"); + ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "height", PROPERTY_HINT_RANGE, "0.001,100.0,0.001,or_greater"), "set_height", "get_height"); ADD_PROPERTY(PropertyInfo(Variant::INT, "radial_segments", PROPERTY_HINT_RANGE, "1,100,1,or_greater"), "set_radial_segments", "get_radial_segments"); ADD_PROPERTY(PropertyInfo(Variant::INT, "rings", PROPERTY_HINT_RANGE, "1,100,1,or_greater"), "set_rings", "get_rings"); } void CapsuleMesh::set_radius(const float p_radius) { radius = p_radius; + if (radius > height * 0.5) { + radius = height * 0.5; + } _request_update(); } @@ -433,13 +436,16 @@ float CapsuleMesh::get_radius() const { return radius; } -void CapsuleMesh::set_mid_height(const float p_mid_height) { - mid_height = p_mid_height; +void CapsuleMesh::set_height(const float p_height) { + height = p_height; + if (radius > height * 0.5) { + height = radius * 2; + } _request_update(); } -float CapsuleMesh::get_mid_height() const { - return mid_height; +float CapsuleMesh::get_height() const { + return height; } void CapsuleMesh::set_radial_segments(const int p_segments) { diff --git a/scene/resources/primitive_meshes.h b/scene/resources/primitive_meshes.h index a3de34d3e35..7915cb0028a 100644 --- a/scene/resources/primitive_meshes.h +++ b/scene/resources/primitive_meshes.h @@ -108,7 +108,7 @@ class CapsuleMesh : public PrimitiveMesh { private: float radius = 1.0; - float mid_height = 1.0; + float height = 3.0; int radial_segments = 64; int rings = 8; @@ -120,8 +120,8 @@ public: void set_radius(const float p_radius); float get_radius() const; - void set_mid_height(const float p_mid_height); - float get_mid_height() const; + void set_height(const float p_height); + float get_height() const; void set_radial_segments(const int p_segments); int get_radial_segments() const; diff --git a/servers/physics_3d/collision_solver_3d_sat.cpp b/servers/physics_3d/collision_solver_3d_sat.cpp index e6a17b3a1a4..6e6a2cb9e78 100644 --- a/servers/physics_3d/collision_solver_3d_sat.cpp +++ b/servers/physics_3d/collision_solver_3d_sat.cpp @@ -848,7 +848,7 @@ static void _collision_sphere_capsule(const Shape3DSW *p_a, const Transform3D &p //capsule sphere 1, sphere - Vector3 capsule_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5); + Vector3 capsule_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); Vector3 capsule_ball_1 = p_transform_b.origin + capsule_axis; @@ -1207,7 +1207,7 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform3D &p_tr // capsule balls, edges of A for (int i = 0; i < 2; i++) { - Vector3 capsule_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5); + Vector3 capsule_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); Vector3 sphere_pos = p_transform_b.origin + ((i == 0) ? capsule_axis : -capsule_axis); @@ -1607,8 +1607,8 @@ static void _collision_capsule_capsule(const Shape3DSW *p_a, const Transform3D & // some values - Vector3 capsule_A_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5); - Vector3 capsule_B_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5); + Vector3 capsule_A_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); + Vector3 capsule_B_axis = p_transform_b.basis.get_axis(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius()); Vector3 capsule_A_ball_1 = p_transform_a.origin + capsule_A_axis; Vector3 capsule_A_ball_2 = p_transform_a.origin - capsule_A_axis; @@ -1679,8 +1679,8 @@ static void _collision_capsule_cylinder(const Shape3DSW *p_a, const Transform3D Vector3 capsule_A_axis = p_transform_a.basis.get_axis(1); - Vector3 capsule_A_ball_1 = p_transform_a.origin + capsule_A_axis * (capsule_A->get_height() * 0.5); - Vector3 capsule_A_ball_2 = p_transform_a.origin - capsule_A_axis * (capsule_A->get_height() * 0.5); + Vector3 capsule_A_ball_1 = p_transform_a.origin + capsule_A_axis * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); + Vector3 capsule_A_ball_2 = p_transform_a.origin - capsule_A_axis * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); if (!separator.test_axis((p_transform_b.origin - capsule_A_ball_1).cross(cylinder_B_axis).cross(cylinder_B_axis).normalized())) { return; @@ -1768,7 +1768,7 @@ static void _collision_capsule_convex_polygon(const Shape3DSW *p_a, const Transf for (int i = 0; i < 2; i++) { // edges of B, capsule cylinder - Vector3 capsule_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5); + Vector3 capsule_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); Vector3 sphere_pos = p_transform_a.origin + ((i == 0) ? capsule_axis : -capsule_axis); @@ -1808,7 +1808,7 @@ static void _collision_capsule_face(const Shape3DSW *p_a, const Transform3D &p_t // edges of B, capsule cylinder - Vector3 capsule_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5); + Vector3 capsule_axis = p_transform_a.basis.get_axis(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius()); for (int i = 0; i < 3; i++) { // edge-cylinder diff --git a/servers/physics_3d/shape_3d_sw.cpp b/servers/physics_3d/shape_3d_sw.cpp index 526a098a05b..a84405de81f 100644 --- a/servers/physics_3d/shape_3d_sw.cpp +++ b/servers/physics_3d/shape_3d_sw.cpp @@ -424,10 +424,10 @@ BoxShape3DSW::BoxShape3DSW() { void CapsuleShape3DSW::project_range(const Vector3 &p_normal, const Transform3D &p_transform, real_t &r_min, real_t &r_max) const { Vector3 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)); @@ -436,10 +436,10 @@ void CapsuleShape3DSW::project_range(const Vector3 &p_normal, const Transform3D Vector3 CapsuleShape3DSW::get_support(const Vector3 &p_normal) const { Vector3 n = p_normal; - 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; return n; } @@ -457,15 +457,15 @@ void CapsuleShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 r_amount = 2; r_type = FEATURE_EDGE; 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_type = FEATURE_POINT; *r_supports = n; @@ -484,7 +484,7 @@ bool CapsuleShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 & // test against cylinder and spheres :-| - collided = Geometry3D::segment_intersects_cylinder(p_begin, p_end, height, radius, &auxres, &auxn, 1); + collided = Geometry3D::segment_intersects_cylinder(p_begin, p_end, height - radius * 2.0, radius, &auxres, &auxn, 1); if (collided) { real_t d = norm.dot(auxres); @@ -496,7 +496,7 @@ bool CapsuleShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 & } } - collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, height * 0.5, 0), radius, &auxres, &auxn); + collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, height * 0.5 - radius, 0), radius, &auxres, &auxn); if (collided) { real_t d = norm.dot(auxres); @@ -508,7 +508,7 @@ bool CapsuleShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 & } } - collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, height * -0.5, 0), radius, &auxres, &auxn); + collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, height * -0.5 + radius, 0), radius, &auxres, &auxn); if (collided) { real_t d = norm.dot(auxres); @@ -529,19 +529,19 @@ bool CapsuleShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 & } bool CapsuleShape3DSW::intersect_point(const Vector3 &p_point) const { - if (Math::abs(p_point.y) < height * 0.5) { + if (Math::abs(p_point.y) < height * 0.5 - radius) { return Vector3(p_point.x, 0, p_point.z).length() < radius; } else { Vector3 p = p_point; - p.y = Math::abs(p.y) - height * 0.5; + p.y = Math::abs(p.y) - height * 0.5 + radius; return p.length() < radius; } } Vector3 CapsuleShape3DSW::get_closest_point_to(const Vector3 &p_point) const { Vector3 s[2] = { - Vector3(0, -height * 0.5, 0), - Vector3(0, height * 0.5, 0), + Vector3(0, -height * 0.5 + radius, 0), + Vector3(0, height * 0.5 - radius, 0), }; Vector3 p = Geometry3D::get_closest_point_to_segment(p_point, s); @@ -566,7 +566,7 @@ Vector3 CapsuleShape3DSW::get_moment_of_inertia(real_t p_mass) const { void CapsuleShape3DSW::_setup(real_t p_height, real_t p_radius) { height = p_height; radius = p_radius; - configure(AABB(Vector3(-radius, -height * 0.5 - radius, -radius), Vector3(radius * 2, height + radius * 2.0, radius * 2))); + configure(AABB(Vector3(-radius, -height * 0.5, -radius), Vector3(radius * 2, height, radius * 2))); } void CapsuleShape3DSW::set_data(const Variant &p_data) { diff --git a/servers/physics_3d/shape_3d_sw.h b/servers/physics_3d/shape_3d_sw.h index dde423ad607..c11c3a08f6b 100644 --- a/servers/physics_3d/shape_3d_sw.h +++ b/servers/physics_3d/shape_3d_sw.h @@ -207,7 +207,7 @@ public: _FORCE_INLINE_ real_t get_height() const { return height; } _FORCE_INLINE_ real_t get_radius() const { return radius; } - virtual real_t get_area() const { return 4.0 / 3.0 * Math_PI * radius * radius * radius + height * Math_PI * radius * radius; } + virtual real_t get_area() const { return 4.0 / 3.0 * Math_PI * radius * radius * radius + (height - radius * 2.0) * Math_PI * radius * radius; } virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CAPSULE; }