diff options
Diffstat (limited to 'servers/physics_3d')
| -rw-r--r-- | servers/physics_3d/collision_object_3d_sw.cpp | 10 | ||||
| -rw-r--r-- | servers/physics_3d/collision_solver_3d_sat.cpp | 710 | ||||
| -rw-r--r-- | servers/physics_3d/collision_solver_3d_sw.cpp | 39 | ||||
| -rw-r--r-- | servers/physics_3d/gjk_epa.cpp | 99 | ||||
| -rw-r--r-- | servers/physics_3d/joints/cone_twist_joint_3d_sw.h | 2 | ||||
| -rw-r--r-- | servers/physics_3d/joints/generic_6dof_joint_3d_sw.h | 2 | ||||
| -rw-r--r-- | servers/physics_3d/joints/hinge_joint_3d_sw.h | 2 | ||||
| -rw-r--r-- | servers/physics_3d/joints/pin_joint_3d_sw.h | 2 | ||||
| -rw-r--r-- | servers/physics_3d/joints/slider_joint_3d_sw.h | 2 | ||||
| -rw-r--r-- | servers/physics_3d/joints_3d_sw.h | 11 | ||||
| -rw-r--r-- | servers/physics_3d/physics_server_3d_sw.cpp | 320 | ||||
| -rw-r--r-- | servers/physics_3d/physics_server_3d_sw.h | 83 | ||||
| -rw-r--r-- | servers/physics_3d/physics_server_3d_wrap_mt.cpp | 140 | ||||
| -rw-r--r-- | servers/physics_3d/physics_server_3d_wrap_mt.h | 422 | ||||
| -rw-r--r-- | servers/physics_3d/shape_3d_sw.cpp | 214 | ||||
| -rw-r--r-- | servers/physics_3d/shape_3d_sw.h | 56 | ||||
| -rw-r--r-- | servers/physics_3d/space_3d_sw.cpp | 12 |
17 files changed, 1870 insertions, 256 deletions
diff --git a/servers/physics_3d/collision_object_3d_sw.cpp b/servers/physics_3d/collision_object_3d_sw.cpp index b06ade5ed3..293a7e6606 100644 --- a/servers/physics_3d/collision_object_3d_sw.cpp +++ b/servers/physics_3d/collision_object_3d_sw.cpp @@ -43,7 +43,7 @@ void CollisionObject3DSW::add_shape(Shape3DSW *p_shape, const Transform &p_trans p_shape->add_owner(this); if (!pending_shape_update_list.in_list()) { - PhysicsServer3DSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); + PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); } //_update_shapes(); //_shapes_changed(); @@ -56,7 +56,7 @@ void CollisionObject3DSW::set_shape(int p_index, Shape3DSW *p_shape) { p_shape->add_owner(this); if (!pending_shape_update_list.in_list()) { - PhysicsServer3DSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); + PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); } //_update_shapes(); //_shapes_changed(); @@ -68,7 +68,7 @@ void CollisionObject3DSW::set_shape_transform(int p_index, const Transform &p_tr shapes.write[p_index].xform = p_transform; shapes.write[p_index].xform_inv = p_transform.affine_inverse(); if (!pending_shape_update_list.in_list()) { - PhysicsServer3DSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); + PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); } //_update_shapes(); //_shapes_changed(); @@ -77,7 +77,7 @@ void CollisionObject3DSW::set_shape_transform(int p_index, const Transform &p_tr void CollisionObject3DSW::set_shape_as_disabled(int p_idx, bool p_enable) { shapes.write[p_idx].disabled = p_enable; if (!pending_shape_update_list.in_list()) { - PhysicsServer3DSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); + PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); } } @@ -106,7 +106,7 @@ void CollisionObject3DSW::remove_shape(int p_index) { shapes.remove(p_index); if (!pending_shape_update_list.in_list()) { - PhysicsServer3DSW::singleton->pending_shape_update_list.add(&pending_shape_update_list); + PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list); } //_update_shapes(); //_shapes_changed(); diff --git a/servers/physics_3d/collision_solver_3d_sat.cpp b/servers/physics_3d/collision_solver_3d_sat.cpp index b8e056f1f4..2a31cf1c22 100644 --- a/servers/physics_3d/collision_solver_3d_sat.cpp +++ b/servers/physics_3d/collision_solver_3d_sat.cpp @@ -31,7 +31,38 @@ #include "collision_solver_3d_sat.h" #include "core/math/geometry_3d.h" -#define _EDGE_IS_VALID_SUPPORT_THRESHOLD 0.02 +#include "gjk_epa.h" + +#define fallback_collision_solver gjk_epa_calculate_penetration + +// Cylinder SAT analytic methods for Cylinder-trimesh and Cylinder-box are based on ODE colliders. + +/* + * Cylinder-trimesh and Cylinder-box colliders by Alen Ladavac + * Ported to ODE by Nguyen Binh + */ + +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ struct _CollectorCallback { CollisionSolver3DSW::CallbackResult callback; @@ -82,6 +113,36 @@ static void _generate_contacts_point_face(const Vector3 *p_points_A, int p_point p_callback->call(*p_points_A, closest_B); } +static void _generate_contacts_point_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 1); + ERR_FAIL_COND(p_point_count_B != 3); +#endif + + const Vector3 &point_A = p_points_A[0]; + + const Vector3 &circle_B_pos = p_points_B[0]; + Vector3 circle_B_line_1 = p_points_B[1] - circle_B_pos; + Vector3 circle_B_line_2 = p_points_B[2] - circle_B_pos; + + real_t circle_B_radius = circle_B_line_1.length(); + Vector3 circle_B_normal = circle_B_line_1.cross(circle_B_line_2).normalized(); + + // Project point onto Circle B plane. + Plane circle_plane(circle_B_pos, circle_B_normal); + Vector3 proj_point_A = circle_plane.project(point_A); + + // Clip point. + Vector3 delta_point_1 = proj_point_A - circle_B_pos; + real_t dist_point_1 = delta_point_1.length_squared(); + if (!Math::is_zero_approx(dist_point_1)) { + dist_point_1 = Math::sqrt(dist_point_1); + proj_point_A = circle_B_pos + delta_point_1 * MIN(dist_point_1, circle_B_radius) / dist_point_1; + } + + p_callback->call(point_A, proj_point_A); +} + static void _generate_contacts_edge_edge(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { #ifdef DEBUG_ENABLED ERR_FAIL_COND(p_point_count_A != 2); @@ -217,36 +278,319 @@ static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_ } } -static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +static void _generate_contacts_face_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A < 2); + ERR_FAIL_COND(p_point_count_B != 3); +#endif + + const Vector3 &circle_B_pos = p_points_B[0]; + Vector3 circle_B_line_1 = p_points_B[1] - circle_B_pos; + Vector3 circle_B_line_2 = p_points_B[2] - circle_B_pos; + + real_t circle_B_radius = circle_B_line_1.length(); + Vector3 circle_B_normal = circle_B_line_1.cross(circle_B_line_2).normalized(); + + Plane circle_plane(circle_B_pos, circle_B_normal); + + bool edge = (p_point_count_A == 2); + + static const int max_clip = 32; + Vector3 contact_points[max_clip]; + int num_points = 0; + + // Clip edges with circle. + for (int i = 0; i < p_point_count_A; i++) { + int i_n = (i + 1) % p_point_count_A; + + // Project edge point in circle plane. + const Vector3 &edge_A_1 = p_points_A[i]; + Vector3 proj_point_1 = circle_plane.project(edge_A_1); + + Vector3 dist_vec = proj_point_1 - circle_B_pos; + real_t dist_sq = dist_vec.length_squared(); + + // Point 1 is inside disk, add as contact point. + if (dist_sq <= circle_B_radius * circle_B_radius) { + //p_callback->call(edge_A_1, proj_point_1); + ERR_FAIL_COND(num_points >= max_clip); + contact_points[num_points] = edge_A_1; + ++num_points; + } + // No need to test point 2 now, as it will be part of the next edge. + + if (edge && i > 0) { + // Done with testing the only two points. + break; + } + + // Project edge point in circle plane. + const Vector3 &edge_A_2 = p_points_A[i_n]; + Vector3 proj_point_2 = circle_plane.project(edge_A_2); + + Vector3 line_vec = proj_point_2 - proj_point_1; + real_t line_length_sq = line_vec.length_squared(); + + // Create a quadratic formula of the form ax^2 + bx + c = 0 + real_t a, b, c; + + a = line_length_sq; + b = 2.0 * dist_vec.dot(line_vec); + c = dist_sq - circle_B_radius * circle_B_radius; + + // Solve for t. + real_t sqrtterm = b * b - 4.0 * a * c; + + // If the term we intend to square root is less than 0 then the answer won't be real, + // so the line doesn't intersect. + if (sqrtterm < 0) { + continue; + } + + sqrtterm = Math::sqrt(sqrtterm); + + Vector3 edge_dir = edge_A_2 - edge_A_1; + + real_t fraction_1 = (-b - sqrtterm) / (2.0 * a); + if ((fraction_1 > 0.0) && (fraction_1 < 1.0)) { + //Vector3 intersection_1 = proj_point_1 + fraction_1 * line_vec; + Vector3 face_point_1 = edge_A_1 + fraction_1 * edge_dir; + //p_callback->call(face_point_1, intersection_1); + ERR_FAIL_COND(num_points >= max_clip); + contact_points[num_points] = face_point_1; + ++num_points; + } + + real_t fraction_2 = (-b + sqrtterm) / (2.0 * a); + if ((fraction_2 > 0.0) && (fraction_2 < 1.0) && !Math::is_equal_approx(fraction_1, fraction_2)) { + //Vector3 intersection_2 = proj_point_1 + fraction_2 * line_vec; + Vector3 face_point_2 = edge_A_1 + fraction_2 * edge_dir; + //p_callback->call(face_point_2, intersection_2); + ERR_FAIL_COND(num_points >= max_clip); + contact_points[num_points] = face_point_2; + ++num_points; + } + } + + // In case of a face, add extra contact points for proper support. + if (!edge) { + Plane plane_A(p_points_A[0], p_points_A[1], p_points_A[2]); + + if (num_points < 3) { + if (num_points == 0) { + // Use 3 arbitrary equidistant points from the circle. + for (int i = 0; i < 3; ++i) { + Vector3 circle_point = circle_B_pos; + circle_point += circle_B_line_1 * Math::cos(2.0 * Math_PI * i / 3.0); + circle_point += circle_B_line_2 * Math::sin(2.0 * Math_PI * i / 3.0); + + Vector3 face_point = plane_A.project(circle_point); + + contact_points[num_points] = face_point; + ++num_points; + } + } else if (num_points == 1) { + Vector3 line_center = circle_B_pos - contact_points[0]; + Vector3 line_tangent = line_center.cross(plane_A.normal); + + Vector3 dir = line_tangent.cross(plane_A.normal).normalized(); + if (line_center.dot(dir) > 0.0) { + // Use 2 equidistant points on the circle inside the face. + line_center.normalize(); + line_tangent.normalize(); + for (int i = 0; i < 2; ++i) { + Vector3 circle_point = circle_B_pos; + circle_point -= line_center * circle_B_radius * Math::cos(2.0 * Math_PI * (i + 1) / 3.0); + circle_point += line_tangent * circle_B_radius * Math::sin(2.0 * Math_PI * (i + 1) / 3.0); + + Vector3 face_point = plane_A.project(circle_point); + + contact_points[num_points] = face_point; + ++num_points; + } + } + // Otherwise the circle touches an edge from the outside, no extra contact point. + } else { // if (num_points == 2) + // Use equidistant 3rd point on the circle inside the face. + Vector3 contacts_line = contact_points[1] - contact_points[0]; + Vector3 dir = contacts_line.cross(plane_A.normal).normalized(); + + Vector3 circle_point = contact_points[0] + 0.5 * contacts_line; + Vector3 line_center = (circle_B_pos - circle_point); + + if (line_center.dot(dir) > 0.0) { + circle_point += dir * (line_center.length() + circle_B_radius); + } else { + circle_point += dir * (circle_B_radius - line_center.length()); + } + + Vector3 face_point = plane_A.project(circle_point); + + contact_points[num_points] = face_point; + ++num_points; + } + } + } + + // Generate contact points. + for (int i = 0; i < num_points; i++) { + const Vector3 &contact_point_A = contact_points[i]; + + real_t d = circle_plane.distance_to(contact_point_A); + Vector3 closest_B = contact_point_A - circle_plane.normal * d; + + if (p_callback->normal.dot(contact_point_A) >= p_callback->normal.dot(closest_B)) { + continue; + } + + p_callback->call(contact_point_A, closest_B); + } +} + +static void _generate_contacts_circle_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) { +#ifdef DEBUG_ENABLED + ERR_FAIL_COND(p_point_count_A != 3); + ERR_FAIL_COND(p_point_count_B != 3); +#endif + + const Vector3 &circle_A_pos = p_points_A[0]; + Vector3 circle_A_line_1 = p_points_A[1] - circle_A_pos; + Vector3 circle_A_line_2 = p_points_A[2] - circle_A_pos; + + real_t circle_A_radius = circle_A_line_1.length(); + Vector3 circle_A_normal = circle_A_line_1.cross(circle_A_line_2).normalized(); + + const Vector3 &circle_B_pos = p_points_B[0]; + Vector3 circle_B_line_1 = p_points_B[1] - circle_B_pos; + Vector3 circle_B_line_2 = p_points_B[2] - circle_B_pos; + + real_t circle_B_radius = circle_B_line_1.length(); + Vector3 circle_B_normal = circle_B_line_1.cross(circle_B_line_2).normalized(); + + static const int max_clip = 4; + Vector3 contact_points[max_clip]; + int num_points = 0; + + Vector3 centers_diff = circle_B_pos - circle_A_pos; + Vector3 norm_proj = circle_A_normal.dot(centers_diff) * circle_A_normal; + Vector3 comp_proj = centers_diff - norm_proj; + real_t proj_dist = comp_proj.length(); + if (!Math::is_zero_approx(proj_dist)) { + comp_proj /= proj_dist; + if ((proj_dist > circle_A_radius - circle_B_radius) && (proj_dist > circle_B_radius - circle_A_radius)) { + // Circles are overlapping, use the 2 points of intersection as contacts. + real_t radius_a_sqr = circle_A_radius * circle_A_radius; + real_t radius_b_sqr = circle_B_radius * circle_B_radius; + real_t d_sqr = proj_dist * proj_dist; + real_t s = (1.0 + (radius_a_sqr - radius_b_sqr) / d_sqr) * 0.5; + real_t h = Math::sqrt(MAX(radius_a_sqr - d_sqr * s * s, 0.0)); + Vector3 midpoint = circle_A_pos + s * comp_proj * proj_dist; + Vector3 h_vec = h * circle_A_normal.cross(comp_proj); + + Vector3 point_A = midpoint + h_vec; + contact_points[num_points] = point_A; + ++num_points; + + point_A = midpoint - h_vec; + contact_points[num_points] = point_A; + ++num_points; + + // Add 2 points from circle A and B along the line between the centers. + point_A = circle_A_pos + comp_proj * circle_A_radius; + contact_points[num_points] = point_A; + ++num_points; + + point_A = circle_B_pos - comp_proj * circle_B_radius - norm_proj; + contact_points[num_points] = point_A; + ++num_points; + } // Otherwise one circle is inside the other one, use 3 arbitrary equidistant points. + } // Otherwise circles are concentric, use 3 arbitrary equidistant points. + + if (num_points == 0) { + // Generate equidistant points. + if (circle_A_radius < circle_B_radius) { + // Circle A inside circle B. + for (int i = 0; i < 3; ++i) { + Vector3 circle_A_point = circle_A_pos; + circle_A_point += circle_A_line_1 * Math::cos(2.0 * Math_PI * i / 3.0); + circle_A_point += circle_A_line_2 * Math::sin(2.0 * Math_PI * i / 3.0); + + contact_points[num_points] = circle_A_point; + ++num_points; + } + } else { + // Circle B inside circle A. + for (int i = 0; i < 3; ++i) { + Vector3 circle_B_point = circle_B_pos; + circle_B_point += circle_B_line_1 * Math::cos(2.0 * Math_PI * i / 3.0); + circle_B_point += circle_B_line_2 * Math::sin(2.0 * Math_PI * i / 3.0); + + Vector3 circle_A_point = circle_B_point - norm_proj; + + contact_points[num_points] = circle_A_point; + ++num_points; + } + } + } + + Plane circle_B_plane(circle_B_pos, circle_B_normal); + + // Generate contact points. + for (int i = 0; i < num_points; i++) { + const Vector3 &contact_point_A = contact_points[i]; + + real_t d = circle_B_plane.distance_to(contact_point_A); + Vector3 closest_B = contact_point_A - circle_B_plane.normal * d; + + if (p_callback->normal.dot(contact_point_A) >= p_callback->normal.dot(closest_B)) { + continue; + } + + p_callback->call(contact_point_A, closest_B); + } +} + +static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_point_count_A, Shape3DSW::FeatureType p_feature_type_A, const Vector3 *p_points_B, int p_point_count_B, Shape3DSW::FeatureType p_feature_type_B, _CollectorCallback *p_callback) { #ifdef DEBUG_ENABLED ERR_FAIL_COND(p_point_count_A < 1); ERR_FAIL_COND(p_point_count_B < 1); #endif - static const GenerateContactsFunc generate_contacts_func_table[3][3] = { + static const GenerateContactsFunc generate_contacts_func_table[4][4] = { { _generate_contacts_point_point, _generate_contacts_point_edge, _generate_contacts_point_face, + _generate_contacts_point_circle, }, { nullptr, _generate_contacts_edge_edge, _generate_contacts_face_face, + _generate_contacts_face_circle, }, { nullptr, nullptr, _generate_contacts_face_face, - } + _generate_contacts_face_circle, + }, + { + nullptr, + nullptr, + nullptr, + _generate_contacts_circle_circle, + }, }; int pointcount_B; int pointcount_A; const Vector3 *points_A; const Vector3 *points_B; + int version_A; + int version_B; - if (p_point_count_A > p_point_count_B) { + if (p_feature_type_A > p_feature_type_B) { //swap p_callback->swap = !p_callback->swap; p_callback->normal = -p_callback->normal; @@ -255,16 +599,17 @@ static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_po pointcount_A = p_point_count_B; points_A = p_points_B; points_B = p_points_A; + version_A = p_feature_type_B; + version_B = p_feature_type_A; } else { pointcount_B = p_point_count_B; pointcount_A = p_point_count_A; points_A = p_points_A; points_B = p_points_B; + version_A = p_feature_type_A; + version_B = p_feature_type_B; } - int version_A = (pointcount_A > 3 ? 3 : pointcount_A) - 1; - int version_B = (pointcount_B > 3 ? 3 : pointcount_B) - 1; - GenerateContactsFunc contacts_func = generate_contacts_func_table[version_A][version_B]; ERR_FAIL_COND(!contacts_func); contacts_func(points_A, pointcount_A, points_B, pointcount_B, p_callback); @@ -346,6 +691,17 @@ public: return true; } + static _FORCE_INLINE_ void test_contact_points(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) { + SeparatorAxisTest<ShapeA, ShapeB, withMargin> *separator = (SeparatorAxisTest<ShapeA, ShapeB, withMargin> *)p_userdata; + Vector3 axis = (p_point_B - p_point_A); + real_t depth = axis.length(); + + // Filter out bogus directions with a treshold and re-testing axis. + if (separator->best_depth - depth > 0.001) { + separator->test_axis(axis / depth); + } + } + _FORCE_INLINE_ void generate_contacts() { // nothing to do, don't generate if (best_axis == Vector3(0.0, 0.0, 0.0)) { @@ -365,7 +721,8 @@ public: Vector3 supports_A[max_supports]; int support_count_A; - shape_A->get_supports(transform_A->basis.xform_inv(-best_axis).normalized(), max_supports, supports_A, support_count_A); + Shape3DSW::FeatureType support_type_A; + shape_A->get_supports(transform_A->basis.xform_inv(-best_axis).normalized(), max_supports, supports_A, support_count_A, support_type_A); for (int i = 0; i < support_count_A; i++) { supports_A[i] = transform_A->xform(supports_A[i]); } @@ -378,7 +735,8 @@ public: Vector3 supports_B[max_supports]; int support_count_B; - shape_B->get_supports(transform_B->basis.xform_inv(best_axis).normalized(), max_supports, supports_B, support_count_B); + Shape3DSW::FeatureType support_type_B; + shape_B->get_supports(transform_B->basis.xform_inv(best_axis).normalized(), max_supports, supports_B, support_count_B, support_type_B); for (int i = 0; i < support_count_B; i++) { supports_B[i] = transform_B->xform(supports_B[i]); } @@ -393,7 +751,7 @@ public: if (callback->prev_axis) { *callback->prev_axis = best_axis; } - _generate_contacts_from_supports(supports_A, support_count_A, supports_B, support_count_B, callback); + _generate_contacts_from_supports(supports_A, support_count_A, support_type_A, supports_B, support_count_B, support_type_B, callback); callback->collided = true; } @@ -529,6 +887,61 @@ static void _collision_sphere_capsule(const Shape3DSW *p_a, const Transform &p_t template <bool withMargin> static void _collision_sphere_cylinder(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a); + const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b); + + SeparatorAxisTest<SphereShape3DSW, CylinderShape3DSW, withMargin> separator(sphere_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + // Cylinder B end caps. + Vector3 cylinder_B_axis = p_transform_b.basis.get_axis(1).normalized(); + if (!separator.test_axis(cylinder_B_axis)) { + return; + } + + Vector3 cylinder_diff = p_transform_b.origin - p_transform_a.origin; + + // Cylinder B lateral surface. + if (!separator.test_axis(cylinder_B_axis.cross(cylinder_diff).cross(cylinder_B_axis).normalized())) { + return; + } + + // Closest point to cylinder caps. + const Vector3 &sphere_center = p_transform_a.origin; + Vector3 cyl_axis = p_transform_b.basis.get_axis(1); + Vector3 cap_axis = p_transform_b.basis.get_axis(0); + real_t height_scale = cyl_axis.length(); + real_t cap_dist = cylinder_B->get_height() * 0.5 * height_scale; + cyl_axis /= height_scale; + real_t radius_scale = cap_axis.length(); + real_t cap_radius = cylinder_B->get_radius() * radius_scale; + + for (int i = 0; i < 2; i++) { + Vector3 cap_dir = ((i == 0) ? cyl_axis : -cyl_axis); + Vector3 cap_pos = p_transform_b.origin + cap_dir * cap_dist; + + Vector3 closest_point; + + Vector3 diff = sphere_center - cap_pos; + Vector3 proj = diff - cap_dir.dot(diff) * cap_dir; + + real_t proj_len = proj.length(); + if (Math::is_zero_approx(proj_len)) { + // Point is equidistant to all circle points. + continue; + } + + closest_point = cap_pos + (cap_radius / proj_len) * proj; + + if (!separator.test_axis((closest_point - sphere_center).normalized())) { + return; + } + } + + separator.generate_contacts(); } template <bool withMargin> @@ -739,7 +1152,7 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_tran // faces of A for (int i = 0; i < 3; i++) { - Vector3 axis = p_transform_a.basis.get_axis(i); + Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); if (!separator.test_axis(axis)) { return; @@ -826,6 +1239,115 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_tran template <bool withMargin> static void _collision_box_cylinder(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a); + const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b); + + SeparatorAxisTest<BoxShape3DSW, CylinderShape3DSW, withMargin> separator(box_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + // Faces of A. + for (int i = 0; i < 3; i++) { + Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + Vector3 cyl_axis = p_transform_b.basis.get_axis(1).normalized(); + + // Cylinder end caps. + { + if (!separator.test_axis(cyl_axis)) { + return; + } + } + + // Edges of A, cylinder lateral surface. + for (int i = 0; i < 3; i++) { + Vector3 box_axis = p_transform_a.basis.get_axis(i); + Vector3 axis = box_axis.cross(cyl_axis); + if (Math::is_zero_approx(axis.length_squared())) { + continue; + } + + if (!separator.test_axis(axis.normalized())) { + return; + } + } + + // Gather points of A. + Vector3 vertices_A[8]; + Vector3 box_extent = box_A->get_half_extents(); + for (int i = 0; i < 2; i++) { + for (int j = 0; j < 2; j++) { + for (int k = 0; k < 2; k++) { + Vector3 extent = box_extent; + extent.x *= (i * 2 - 1); + extent.y *= (j * 2 - 1); + extent.z *= (k * 2 - 1); + Vector3 &point = vertices_A[i * 2 * 2 + j * 2 + k]; + point = p_transform_a.origin; + for (int l = 0; l < 3; l++) { + point += p_transform_a.basis.get_axis(l) * extent[l]; + } + } + } + } + + // Points of A, cylinder lateral surface. + for (int i = 0; i < 8; i++) { + const Vector3 &point = vertices_A[i]; + Vector3 axis = Plane(cyl_axis, 0).project(point).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + // Edges of A, cylinder end caps rim. + int edges_start_A[12] = { 0, 2, 4, 6, 0, 1, 4, 5, 0, 1, 2, 3 }; + int edges_end_A[12] = { 1, 3, 5, 7, 2, 3, 6, 7, 4, 5, 6, 7 }; + + Vector3 cap_axis = cyl_axis * (cylinder_B->get_height() * 0.5); + + for (int i = 0; i < 2; i++) { + Vector3 cap_pos = p_transform_b.origin + ((i == 0) ? cap_axis : -cap_axis); + + for (int e = 0; e < 12; e++) { + const Vector3 &edge_start = vertices_A[edges_start_A[e]]; + const Vector3 &edge_end = vertices_A[edges_end_A[e]]; + + Vector3 edge_dir = (edge_end - edge_start); + edge_dir.normalize(); + + real_t edge_dot = edge_dir.dot(cyl_axis); + if (Math::is_zero_approx(edge_dot)) { + // Edge is perpendicular to cylinder axis. + continue; + } + + // Calculate intersection between edge and circle plane. + Vector3 edge_diff = cap_pos - edge_start; + real_t diff_dot = edge_diff.dot(cyl_axis); + Vector3 intersection = edge_start + edge_dir * diff_dot / edge_dot; + + // Calculate tangent that touches intersection. + Vector3 tangent = (cap_pos - intersection).cross(cyl_axis); + + // Axis is orthogonal both to tangent and edge direction. + Vector3 axis = tangent.cross(edge_dir); + + if (!separator.test_axis(axis.normalized())) { + return; + } + } + } + + separator.generate_contacts(); } template <bool withMargin> @@ -1111,6 +1633,19 @@ static void _collision_capsule_capsule(const Shape3DSW *p_a, const Transform &p_ template <bool withMargin> static void _collision_capsule_cylinder(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const CapsuleShape3DSW *capsule_A = static_cast<const CapsuleShape3DSW *>(p_a); + const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b); + + SeparatorAxisTest<CapsuleShape3DSW, CylinderShape3DSW, withMargin> separator(capsule_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + CollisionSolver3DSW::CallbackResult callback = SeparatorAxisTest<CapsuleShape3DSW, CylinderShape3DSW, withMargin>::test_contact_points; + + // Fallback to generic algorithm to find the best separating axis. + if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator)) { + return; + } + + separator.generate_contacts(); } template <bool withMargin> @@ -1236,14 +1771,165 @@ static void _collision_capsule_face(const Shape3DSW *p_a, const Transform &p_tra template <bool withMargin> static void _collision_cylinder_cylinder(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const CylinderShape3DSW *cylinder_A = static_cast<const CylinderShape3DSW *>(p_a); + const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b); + + SeparatorAxisTest<CylinderShape3DSW, CylinderShape3DSW, withMargin> separator(cylinder_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + Vector3 cylinder_A_axis = p_transform_a.basis.get_axis(1); + Vector3 cylinder_B_axis = p_transform_b.basis.get_axis(1); + + if (!separator.test_previous_axis()) { + return; + } + + // Cylinder A end caps. + if (!separator.test_axis(cylinder_A_axis.normalized())) { + return; + } + + // Cylinder B end caps. + if (!separator.test_axis(cylinder_A_axis.normalized())) { + return; + } + + Vector3 cylinder_diff = p_transform_b.origin - p_transform_a.origin; + + // Cylinder A lateral surface. + if (!separator.test_axis(cylinder_A_axis.cross(cylinder_diff).cross(cylinder_A_axis).normalized())) { + return; + } + + // Cylinder B lateral surface. + if (!separator.test_axis(cylinder_B_axis.cross(cylinder_diff).cross(cylinder_B_axis).normalized())) { + return; + } + + real_t proj = cylinder_A_axis.cross(cylinder_B_axis).cross(cylinder_B_axis).dot(cylinder_A_axis); + if (Math::is_zero_approx(proj)) { + // Parallel cylinders, handle with specific axes only. + // Note: GJKEPA with no margin can lead to degenerate cases in this situation. + separator.generate_contacts(); + return; + } + + CollisionSolver3DSW::CallbackResult callback = SeparatorAxisTest<CylinderShape3DSW, CylinderShape3DSW, withMargin>::test_contact_points; + + // Fallback to generic algorithm to find the best separating axis. + if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator)) { + return; + } + + separator.generate_contacts(); } template <bool withMargin> static void _collision_cylinder_convex_polygon(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const CylinderShape3DSW *cylinder_A = static_cast<const CylinderShape3DSW *>(p_a); + const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b); + + SeparatorAxisTest<CylinderShape3DSW, ConvexPolygonShape3DSW, withMargin> separator(cylinder_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + CollisionSolver3DSW::CallbackResult callback = SeparatorAxisTest<CylinderShape3DSW, ConvexPolygonShape3DSW, withMargin>::test_contact_points; + + // Fallback to generic algorithm to find the best separating axis. + if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator)) { + return; + } + + separator.generate_contacts(); } template <bool withMargin> static void _collision_cylinder_face(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) { + const CylinderShape3DSW *cylinder_A = static_cast<const CylinderShape3DSW *>(p_a); + const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b); + + SeparatorAxisTest<CylinderShape3DSW, FaceShape3DSW, withMargin> separator(cylinder_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b); + + if (!separator.test_previous_axis()) { + return; + } + + Vector3 vertex[3] = { + p_transform_b.xform(face_B->vertex[0]), + p_transform_b.xform(face_B->vertex[1]), + p_transform_b.xform(face_B->vertex[2]), + }; + + // Face B normal. + if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) { + return; + } + + Vector3 cyl_axis = p_transform_a.basis.get_axis(1).normalized(); + + // Cylinder end caps. + { + if (!separator.test_axis(cyl_axis)) { + return; + } + } + + // Edges of B, cylinder lateral surface. + for (int i = 0; i < 3; i++) { + Vector3 edge_axis = vertex[i] - vertex[(i + 1) % 3]; + Vector3 axis = edge_axis.cross(cyl_axis); + if (Math::is_zero_approx(axis.length_squared())) { + continue; + } + + if (!separator.test_axis(axis.normalized())) { + return; + } + } + + // Points of B, cylinder lateral surface. + for (int i = 0; i < 3; i++) { + const Vector3 &point = vertex[i]; + Vector3 axis = Plane(cyl_axis, 0).project(point).normalized(); + + if (!separator.test_axis(axis)) { + return; + } + } + + // Edges of B, cylinder end caps rim. + Vector3 cap_axis = cyl_axis * (cylinder_A->get_height() * 0.5); + + for (int i = 0; i < 2; i++) { + Vector3 cap_pos = p_transform_a.origin + ((i == 0) ? cap_axis : -cap_axis); + + for (int j = 0; j < 3; j++) { + const Vector3 &edge_start = vertex[j]; + const Vector3 &edge_end = vertex[(j + 1) % 3]; + Vector3 edge_dir = edge_end - edge_start; + edge_dir.normalize(); + + real_t edge_dot = edge_dir.dot(cyl_axis); + if (Math::is_zero_approx(edge_dot)) { + // Edge is perpendicular to cylinder axis. + continue; + } + + // Calculate intersection between edge and circle plane. + Vector3 edge_diff = cap_pos - edge_start; + real_t diff_dot = edge_diff.dot(cyl_axis); + Vector3 intersection = edge_start + edge_dir * diff_dot / edge_dot; + + // Calculate tangent that touches intersection. + Vector3 tangent = (cap_pos - intersection).cross(cyl_axis); + + // Axis is orthogonal both to tangent and edge direction. + Vector3 axis = tangent.cross(edge_dir); + + if (!separator.test_axis(axis.normalized())) { + return; + } + } + } + + separator.generate_contacts(); } template <bool withMargin> diff --git a/servers/physics_3d/collision_solver_3d_sw.cpp b/servers/physics_3d/collision_solver_3d_sw.cpp index 1150696b84..fd9ea00d92 100644 --- a/servers/physics_3d/collision_solver_3d_sw.cpp +++ b/servers/physics_3d/collision_solver_3d_sw.cpp @@ -46,8 +46,24 @@ bool CollisionSolver3DSW::solve_static_plane(const Shape3DSW *p_shape_A, const T static const int max_supports = 16; Vector3 supports[max_supports]; int support_count; - - p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count); + Shape3DSW::FeatureType support_type; + p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count, support_type); + + if (support_type == Shape3DSW::FEATURE_CIRCLE) { + ERR_FAIL_COND_V(support_count != 3, false); + + Vector3 circle_pos = supports[0]; + Vector3 circle_axis_1 = supports[1] - circle_pos; + Vector3 circle_axis_2 = supports[2] - circle_pos; + + // Use 3 equidistant points on the circle. + for (int i = 0; i < 3; ++i) { + Vector3 vertex_pos = circle_pos; + vertex_pos += circle_axis_1 * Math::cos(2.0 * Math_PI * i / 3.0); + vertex_pos += circle_axis_2 * Math::sin(2.0 * Math_PI * i / 3.0); + supports[i] = vertex_pos; + } + } bool found = false; @@ -265,8 +281,25 @@ bool CollisionSolver3DSW::solve_distance_plane(const Shape3DSW *p_shape_A, const static const int max_supports = 16; Vector3 supports[max_supports]; int support_count; + Shape3DSW::FeatureType support_type; + + p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count, support_type); - p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count); + if (support_type == Shape3DSW::FEATURE_CIRCLE) { + ERR_FAIL_COND_V(support_count != 3, false); + + Vector3 circle_pos = supports[0]; + Vector3 circle_axis_1 = supports[1] - circle_pos; + Vector3 circle_axis_2 = supports[2] - circle_pos; + + // Use 3 equidistant points on the circle. + for (int i = 0; i < 3; ++i) { + Vector3 vertex_pos = circle_pos; + vertex_pos += circle_axis_1 * Math::cos(2.0 * Math_PI * i / 3.0); + vertex_pos += circle_axis_2 * Math::sin(2.0 * Math_PI * i / 3.0); + supports[i] = vertex_pos; + } + } bool collided = false; Vector3 closest; diff --git a/servers/physics_3d/gjk_epa.cpp b/servers/physics_3d/gjk_epa.cpp index dafd2feb8b..aa7c11eec5 100644 --- a/servers/physics_3d/gjk_epa.cpp +++ b/servers/physics_3d/gjk_epa.cpp @@ -64,7 +64,7 @@ GJK-EPA collision solver by Nathanael Presson, 2008 /* GJK */ #define GJK_MAX_ITERATIONS 128 -#define GJK_ACCURARY ((real_t)0.0001) +#define GJK_ACCURACY ((real_t)0.0001) #define GJK_MIN_DISTANCE ((real_t)0.0001) #define GJK_DUPLICATED_EPS ((real_t)0.0001) #define GJK_SIMPLEX2_EPS ((real_t)0.0) @@ -72,10 +72,13 @@ GJK-EPA collision solver by Nathanael Presson, 2008 #define GJK_SIMPLEX4_EPS ((real_t)0.0) /* EPA */ -#define EPA_MAX_VERTICES 64 +#define EPA_MAX_VERTICES 128 #define EPA_MAX_FACES (EPA_MAX_VERTICES*2) #define EPA_MAX_ITERATIONS 255 -#define EPA_ACCURACY ((real_t)0.0001) +// -- GODOT start -- +//#define EPA_ACCURACY ((real_t)0.0001) +#define EPA_ACCURACY ((real_t)0.00001) +// -- GODOT end -- #define EPA_FALLBACK (10*EPA_ACCURACY) #define EPA_PLANE_EPS ((real_t)0.00001) #define EPA_INSIDE_EPS ((real_t)0.01) @@ -237,7 +240,7 @@ struct GJK /* Check for termination */ const real_t omega=vec3_dot(m_ray,w)/rl; alpha=MAX(omega,alpha); - if(((rl-alpha)-(GJK_ACCURARY*rl))<=0) + if(((rl-alpha)-(GJK_ACCURACY*rl))<=0) {/* Return old simplex */ removevertice(m_simplices[m_current]); break; @@ -466,7 +469,7 @@ struct GJK if(ng&&(Math::abs(vl)>GJK_SIMPLEX4_EPS)) { real_t mindist=-1; - real_t subw[3]; + real_t subw[3] = {0.f, 0.f, 0.f}; U subm=0; for(U i=0;i<3;++i) { @@ -512,7 +515,6 @@ struct GJK { Vector3 n; real_t d; - real_t p; sSV* c[3]; sFace* f[3]; sFace* l[2]; @@ -661,8 +663,7 @@ struct GJK remove(m_hull,best); append(m_stock,best); best=findbest(); - if(best->p>=outer.p) { outer=*best; -} + outer=*best; } else { m_status=eStatus::InvalidHull;break; } } else { m_status=eStatus::AccuraryReached;break; } } else { m_status=eStatus::OutOfVertices;break; } @@ -688,24 +689,54 @@ struct GJK } } /* Fallback */ - m_status = eStatus::FallBack; - m_normal = -guess; - const real_t nl=m_normal.length(); - if(nl>0) { - m_normal = m_normal/nl; + m_status = eStatus::FallBack; + m_normal = -guess; + const real_t nl = m_normal.length(); + if (nl > 0) { + m_normal = m_normal/nl; } else { - m_normal = Vector3(1,0,0); -} + m_normal = Vector3(1,0,0); + } m_depth = 0; m_result.rank=1; m_result.c[0]=simplex.c[0]; m_result.p[0]=1; return(m_status); } + + bool getedgedist(sFace* face, sSV* a, sSV* b, real_t& dist) + { + const Vector3 ba = b->w - a->w; + const Vector3 n_ab = vec3_cross(ba, face->n); // Outward facing edge normal direction, on triangle plane + const real_t a_dot_nab = vec3_dot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required + + if (a_dot_nab < 0) { + // Outside of edge a->b + const real_t ba_l2 = ba.length_squared(); + const real_t a_dot_ba = vec3_dot(a->w, ba); + const real_t b_dot_ba = vec3_dot(b->w, ba); + + if (a_dot_ba > 0) { + // Pick distance vertex a + dist = a->w.length(); + } else if (b_dot_ba < 0) { + // Pick distance vertex b + dist = b->w.length(); + } else { + // Pick distance to edge a->b + const real_t a_dot_b = vec3_dot(a->w, b->w); + dist = Math::sqrt(MAX((a->w.length_squared() * b->w.length_squared() - a_dot_b * a_dot_b) / ba_l2, 0.0)); + } + + return true; + } + + return false; + } + sFace* newface(sSV* a,sSV* b,sSV* c,bool forced) { - if(m_stock.root) - { + if (m_stock.root) { sFace* face=m_stock.root; remove(m_stock,face); append(m_hull,face); @@ -716,23 +747,23 @@ struct GJK face->n = vec3_cross(b->w-a->w,c->w-a->w); const real_t l=face->n.length(); const bool v=l>EPA_ACCURACY; - face->p = MIN(MIN( - vec3_dot(a->w,vec3_cross(face->n,a->w-b->w)), - vec3_dot(b->w,vec3_cross(face->n,b->w-c->w))), - vec3_dot(c->w,vec3_cross(face->n,c->w-a->w))) / - (v?l:1); - face->p = face->p>=-EPA_INSIDE_EPS?0:face->p; - if(v) - { - face->d = vec3_dot(a->w,face->n)/l; + if (v) { + if (!(getedgedist(face, a, b, face->d) || + getedgedist(face, b, c, face->d) || + getedgedist(face, c, a, face->d))) { + // Origin projects to the interior of the triangle + // Use distance to triangle plane + face->d = vec3_dot(a->w, face->n) / l; + } face->n /= l; - if(forced||(face->d>=-EPA_PLANE_EPS)) - { + if (forced||(face->d>=-EPA_PLANE_EPS)) { return(face); - } else { m_status=eStatus::NonConvex; -} - } else { m_status=eStatus::Degenerated; -} + } else { + m_status=eStatus::NonConvex; + } + } else { + m_status=eStatus::Degenerated; + } remove(m_hull,face); append(m_stock,face); return(nullptr); @@ -747,15 +778,13 @@ struct GJK { sFace* minf=m_hull.root; real_t mind=minf->d*minf->d; - real_t maxp=minf->p; for(sFace* f=minf->l[1];f;f=f->l[1]) { const real_t sqd=f->d*f->d; - if((f->p>=maxp)&&(sqd<mind)) + if(sqd<mind) { minf=f; mind=sqd; - maxp=f->p; } } return(minf); diff --git a/servers/physics_3d/joints/cone_twist_joint_3d_sw.h b/servers/physics_3d/joints/cone_twist_joint_3d_sw.h index c38edc5737..4e4d4e7f0c 100644 --- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.h +++ b/servers/physics_3d/joints/cone_twist_joint_3d_sw.h @@ -102,7 +102,7 @@ public: bool m_solveSwingLimit; public: - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_CONE_TWIST; } + virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_CONE_TWIST; } virtual bool setup(real_t p_timestep); virtual void solve(real_t p_timestep); diff --git a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h index 2ae6fe85fa..d61a033231 100644 --- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h +++ b/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h @@ -234,7 +234,7 @@ protected: public: Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA); - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_6DOF; } + virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_6DOF; } virtual bool setup(real_t p_timestep); virtual void solve(real_t p_timestep); diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.h b/servers/physics_3d/joints/hinge_joint_3d_sw.h index 028a8b8c72..b6117aa0bc 100644 --- a/servers/physics_3d/joints/hinge_joint_3d_sw.h +++ b/servers/physics_3d/joints/hinge_joint_3d_sw.h @@ -96,7 +96,7 @@ class HingeJoint3DSW : public Joint3DSW { real_t m_appliedImpulse; public: - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_HINGE; } + virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_HINGE; } virtual bool setup(real_t p_step); virtual void solve(real_t p_step); diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.h b/servers/physics_3d/joints/pin_joint_3d_sw.h index e28fbec6cd..1875983527 100644 --- a/servers/physics_3d/joints/pin_joint_3d_sw.h +++ b/servers/physics_3d/joints/pin_joint_3d_sw.h @@ -74,7 +74,7 @@ class PinJoint3DSW : public Joint3DSW { Vector3 m_pivotInB; public: - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_PIN; } + virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_PIN; } virtual bool setup(real_t p_step); virtual void solve(real_t p_step); diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.h b/servers/physics_3d/joints/slider_joint_3d_sw.h index 196e60d19d..f52f6ace27 100644 --- a/servers/physics_3d/joints/slider_joint_3d_sw.h +++ b/servers/physics_3d/joints/slider_joint_3d_sw.h @@ -243,7 +243,7 @@ public: bool setup(real_t p_step); void solve(real_t p_step); - virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_SLIDER; } + virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_SLIDER; } }; #endif // SLIDER_JOINT_SW_H diff --git a/servers/physics_3d/joints_3d_sw.h b/servers/physics_3d/joints_3d_sw.h index cad05b6702..1fe573c69e 100644 --- a/servers/physics_3d/joints_3d_sw.h +++ b/servers/physics_3d/joints_3d_sw.h @@ -36,7 +36,16 @@ class Joint3DSW : public Constraint3DSW { public: - virtual PhysicsServer3D::JointType get_type() const = 0; + virtual bool setup(real_t p_step) { return false; } + virtual void solve(real_t p_step) {} + + void copy_settings_from(Joint3DSW *p_joint) { + set_self(p_joint->get_self()); + set_priority(p_joint->get_priority()); + disable_collisions_between_bodies(p_joint->is_disabled_collisions_between_bodies()); + } + + virtual PhysicsServer3D::JointType get_type() const { return PhysicsServer3D::JOINT_TYPE_MAX; } _FORCE_INLINE_ Joint3DSW(Body3DSW **p_body_ptr = nullptr, int p_body_count = 0) : Constraint3DSW(p_body_ptr, p_body_count) { } diff --git a/servers/physics_3d/physics_server_3d_sw.cpp b/servers/physics_3d/physics_server_3d_sw.cpp index b554a23bf2..735e9094d2 100644 --- a/servers/physics_3d/physics_server_3d_sw.cpp +++ b/servers/physics_3d/physics_server_3d_sw.cpp @@ -43,47 +43,63 @@ #define FLUSH_QUERY_CHECK(m_object) \ ERR_FAIL_COND_MSG(m_object->get_space() && flushing_queries, "Can't change this state while flushing queries. Use call_deferred() or set_deferred() to change monitoring state instead."); -RID PhysicsServer3DSW::shape_create(ShapeType p_shape) { - Shape3DSW *shape = nullptr; - switch (p_shape) { - case SHAPE_PLANE: { - shape = memnew(PlaneShape3DSW); - } break; - case SHAPE_RAY: { - shape = memnew(RayShape3DSW); - } break; - case SHAPE_SPHERE: { - shape = memnew(SphereShape3DSW); - } break; - case SHAPE_BOX: { - shape = memnew(BoxShape3DSW); - } break; - case SHAPE_CAPSULE: { - shape = memnew(CapsuleShape3DSW); - } break; - case SHAPE_CYLINDER: { - ERR_FAIL_V_MSG(RID(), "CylinderShape3D is not supported in GodotPhysics3D. Please switch to Bullet in the Project Settings."); - } break; - case SHAPE_CONVEX_POLYGON: { - shape = memnew(ConvexPolygonShape3DSW); - } break; - case SHAPE_CONCAVE_POLYGON: { - shape = memnew(ConcavePolygonShape3DSW); - } break; - case SHAPE_HEIGHTMAP: { - shape = memnew(HeightMapShape3DSW); - } break; - case SHAPE_CUSTOM: { - ERR_FAIL_V(RID()); - - } break; - } - - RID id = shape_owner.make_rid(shape); - shape->set_self(id); - - return id; -}; +RID PhysicsServer3DSW::plane_shape_create() { + Shape3DSW *shape = memnew(PlaneShape3DSW); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID PhysicsServer3DSW::ray_shape_create() { + Shape3DSW *shape = memnew(RayShape3DSW); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID PhysicsServer3DSW::sphere_shape_create() { + Shape3DSW *shape = memnew(SphereShape3DSW); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID PhysicsServer3DSW::box_shape_create() { + Shape3DSW *shape = memnew(BoxShape3DSW); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID PhysicsServer3DSW::capsule_shape_create() { + Shape3DSW *shape = memnew(CapsuleShape3DSW); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID PhysicsServer3DSW::cylinder_shape_create() { + Shape3DSW *shape = memnew(CylinderShape3DSW); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID PhysicsServer3DSW::convex_polygon_shape_create() { + Shape3DSW *shape = memnew(ConvexPolygonShape3DSW); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID PhysicsServer3DSW::concave_polygon_shape_create() { + Shape3DSW *shape = memnew(ConcavePolygonShape3DSW); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID PhysicsServer3DSW::heightmap_shape_create() { + Shape3DSW *shape = memnew(HeightMapShape3DSW); + RID rid = shape_owner.make_rid(shape); + shape->set_self(rid); + return rid; +} +RID PhysicsServer3DSW::custom_shape_create() { + ERR_FAIL_V(RID()); +} void PhysicsServer3DSW::shape_set_data(RID p_shape, const Variant &p_data) { Shape3DSW *shape = shape_owner.getornull(p_shape); @@ -174,7 +190,7 @@ real_t PhysicsServer3DSW::space_get_param(RID p_space, SpaceParameter p_param) c PhysicsDirectSpaceState3D *PhysicsServer3DSW::space_get_direct_state(RID p_space) { Space3DSW *space = space_owner.getornull(p_space); ERR_FAIL_COND_V(!space, nullptr); - ERR_FAIL_COND_V_MSG(space->is_locked(), nullptr, "Space state is inaccessible right now, wait for iteration or physics process notification."); + ERR_FAIL_COND_V_MSG((using_threads && !doing_sync) || space->is_locked(), nullptr, "Space state is inaccessible right now, wait for iteration or physics process notification."); return space->get_direct_state(); } @@ -413,13 +429,6 @@ void PhysicsServer3DSW::area_set_ray_pickable(RID p_area, bool p_enable) { area->set_ray_pickable(p_enable); } -bool PhysicsServer3DSW::area_is_ray_pickable(RID p_area) const { - Area3DSW *area = area_owner.getornull(p_area); - ERR_FAIL_COND_V(!area, false); - - return area->is_ray_pickable(); -} - void PhysicsServer3DSW::area_set_area_monitor_callback(RID p_area, Object *p_receiver, const StringName &p_method) { Area3DSW *area = area_owner.getornull(p_area); ERR_FAIL_COND(!area); @@ -429,14 +438,8 @@ void PhysicsServer3DSW::area_set_area_monitor_callback(RID p_area, Object *p_rec /* BODY API */ -RID PhysicsServer3DSW::body_create(BodyMode p_mode, bool p_init_sleeping) { +RID PhysicsServer3DSW::body_create() { Body3DSW *body = memnew(Body3DSW); - if (p_mode != BODY_MODE_RIGID) { - body->set_mode(p_mode); - } - if (p_init_sleeping) { - body->set_state(BODY_STATE_SLEEPING, p_init_sleeping); - } RID rid = body_owner.make_rid(body); body->set_self(rid); return rid; @@ -857,12 +860,6 @@ void PhysicsServer3DSW::body_set_ray_pickable(RID p_body, bool p_enable) { body->set_ray_pickable(p_enable); } -bool PhysicsServer3DSW::body_is_ray_pickable(RID p_body) const { - Body3DSW *body = body_owner.getornull(p_body); - ERR_FAIL_COND_V(!body, false); - return body->is_ray_pickable(); -} - bool PhysicsServer3DSW::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result, bool p_exclude_raycast_shapes) { Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, false); @@ -886,6 +883,8 @@ int PhysicsServer3DSW::body_test_ray_separation(RID p_body, const Transform &p_t } PhysicsDirectBodyState3D *PhysicsServer3DSW::body_get_direct_state(RID p_body) { + ERR_FAIL_COND_V_MSG((using_threads && !doing_sync), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); + Body3DSW *body = body_owner.getornull(p_body); ERR_FAIL_COND_V(!body, nullptr); ERR_FAIL_COND_V_MSG(body->get_space()->is_locked(), nullptr, "Body state is inaccessible right now, wait for iteration or physics process notification."); @@ -896,30 +895,52 @@ PhysicsDirectBodyState3D *PhysicsServer3DSW::body_get_direct_state(RID p_body) { /* JOINT API */ -RID PhysicsServer3DSW::joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) { +RID PhysicsServer3DSW::joint_create() { + Joint3DSW *joint = memnew(Joint3DSW); + RID rid = joint_owner.make_rid(joint); + joint->set_self(rid); + return rid; +} + +void PhysicsServer3DSW::joint_clear(RID p_joint) { + Joint3DSW *joint = joint_owner.getornull(p_joint); + if (joint->get_type() != JOINT_TYPE_MAX) { + Joint3DSW *empty_joint = memnew(Joint3DSW); + empty_joint->copy_settings_from(joint); + + joint_owner.replace(p_joint, empty_joint); + memdelete(joint); + } +} + +void PhysicsServer3DSW::joint_make_pin(RID p_joint, RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) { Body3DSW *body_A = body_owner.getornull(p_body_A); - ERR_FAIL_COND_V(!body_A, RID()); + ERR_FAIL_COND(!body_A); if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(), RID()); + ERR_FAIL_COND(!body_A->get_space()); p_body_B = body_A->get_space()->get_static_global_body(); } Body3DSW *body_B = body_owner.getornull(p_body_B); - ERR_FAIL_COND_V(!body_B, RID()); + ERR_FAIL_COND(!body_B); + + ERR_FAIL_COND(body_A == body_B); - ERR_FAIL_COND_V(body_A == body_B, RID()); + Joint3DSW *prev_joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); Joint3DSW *joint = memnew(PinJoint3DSW(body_A, p_local_A, body_B, p_local_B)); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; + + joint->copy_settings_from(prev_joint); + joint_owner.replace(p_joint, joint); + memdelete(prev_joint); } void PhysicsServer3DSW::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_PIN); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN); PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); pin_joint->set_param(p_param, p_value); } @@ -927,7 +948,7 @@ void PhysicsServer3DSW::pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t PhysicsServer3DSW::pin_joint_get_param(RID p_joint, PinJointParam p_param) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, 0); - ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, 0); PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); return pin_joint->get_param(p_param); } @@ -935,7 +956,7 @@ real_t PhysicsServer3DSW::pin_joint_get_param(RID p_joint, PinJointParam p_param void PhysicsServer3DSW::pin_joint_set_local_a(RID p_joint, const Vector3 &p_A) { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_PIN); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN); PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); pin_joint->set_pos_a(p_A); } @@ -943,7 +964,7 @@ void PhysicsServer3DSW::pin_joint_set_local_a(RID p_joint, const Vector3 &p_A) { Vector3 PhysicsServer3DSW::pin_joint_get_local_a(RID p_joint) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, Vector3()); - ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, Vector3()); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, Vector3()); PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); return pin_joint->get_position_a(); } @@ -951,7 +972,7 @@ Vector3 PhysicsServer3DSW::pin_joint_get_local_a(RID p_joint) const { void PhysicsServer3DSW::pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_PIN); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_PIN); PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); pin_joint->set_pos_b(p_B); } @@ -959,55 +980,63 @@ void PhysicsServer3DSW::pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) { Vector3 PhysicsServer3DSW::pin_joint_get_local_b(RID p_joint) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, Vector3()); - ERR_FAIL_COND_V(joint->get_type() != JOINT_PIN, Vector3()); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_PIN, Vector3()); PinJoint3DSW *pin_joint = static_cast<PinJoint3DSW *>(joint); return pin_joint->get_position_b(); } -RID PhysicsServer3DSW::joint_create_hinge(RID p_body_A, const Transform &p_frame_A, RID p_body_B, const Transform &p_frame_B) { +void PhysicsServer3DSW::joint_make_hinge(RID p_joint, RID p_body_A, const Transform &p_frame_A, RID p_body_B, const Transform &p_frame_B) { Body3DSW *body_A = body_owner.getornull(p_body_A); - ERR_FAIL_COND_V(!body_A, RID()); + ERR_FAIL_COND(!body_A); if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(), RID()); + ERR_FAIL_COND(!body_A->get_space()); p_body_B = body_A->get_space()->get_static_global_body(); } Body3DSW *body_B = body_owner.getornull(p_body_B); - ERR_FAIL_COND_V(!body_B, RID()); + ERR_FAIL_COND(!body_B); + + ERR_FAIL_COND(body_A == body_B); - ERR_FAIL_COND_V(body_A == body_B, RID()); + Joint3DSW *prev_joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); Joint3DSW *joint = memnew(HingeJoint3DSW(body_A, body_B, p_frame_A, p_frame_B)); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; + + joint->copy_settings_from(prev_joint); + joint_owner.replace(p_joint, joint); + memdelete(prev_joint); } -RID PhysicsServer3DSW::joint_create_hinge_simple(RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B) { +void PhysicsServer3DSW::joint_make_hinge_simple(RID p_joint, RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B) { Body3DSW *body_A = body_owner.getornull(p_body_A); - ERR_FAIL_COND_V(!body_A, RID()); + ERR_FAIL_COND(!body_A); if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(), RID()); + ERR_FAIL_COND(!body_A->get_space()); p_body_B = body_A->get_space()->get_static_global_body(); } Body3DSW *body_B = body_owner.getornull(p_body_B); - ERR_FAIL_COND_V(!body_B, RID()); + ERR_FAIL_COND(!body_B); - ERR_FAIL_COND_V(body_A == body_B, RID()); + ERR_FAIL_COND(body_A == body_B); + + Joint3DSW *prev_joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); Joint3DSW *joint = memnew(HingeJoint3DSW(body_A, body_B, p_pivot_A, p_pivot_B, p_axis_A, p_axis_B)); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; + + joint->copy_settings_from(prev_joint); + joint_owner.replace(p_joint, joint); + memdelete(prev_joint); } void PhysicsServer3DSW::hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t p_value) { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_HINGE); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_HINGE); HingeJoint3DSW *hinge_joint = static_cast<HingeJoint3DSW *>(joint); hinge_joint->set_param(p_param, p_value); } @@ -1015,7 +1044,7 @@ void PhysicsServer3DSW::hinge_joint_set_param(RID p_joint, HingeJointParam p_par real_t PhysicsServer3DSW::hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, 0); - ERR_FAIL_COND_V(joint->get_type() != JOINT_HINGE, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_HINGE, 0); HingeJoint3DSW *hinge_joint = static_cast<HingeJoint3DSW *>(joint); return hinge_joint->get_param(p_param); } @@ -1023,7 +1052,7 @@ real_t PhysicsServer3DSW::hinge_joint_get_param(RID p_joint, HingeJointParam p_p void PhysicsServer3DSW::hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value) { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_HINGE); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_HINGE); HingeJoint3DSW *hinge_joint = static_cast<HingeJoint3DSW *>(joint); hinge_joint->set_flag(p_flag, p_value); } @@ -1031,7 +1060,7 @@ void PhysicsServer3DSW::hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool PhysicsServer3DSW::hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, false); - ERR_FAIL_COND_V(joint->get_type() != JOINT_HINGE, false); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_HINGE, false); HingeJoint3DSW *hinge_joint = static_cast<HingeJoint3DSW *>(joint); return hinge_joint->get_flag(p_flag); } @@ -1077,34 +1106,38 @@ bool PhysicsServer3DSW::joint_is_disabled_collisions_between_bodies(RID p_joint) PhysicsServer3DSW::JointType PhysicsServer3DSW::joint_get_type(RID p_joint) const { Joint3DSW *joint = joint_owner.getornull(p_joint); - ERR_FAIL_COND_V(!joint, JOINT_PIN); + ERR_FAIL_COND_V(!joint, JOINT_TYPE_PIN); return joint->get_type(); } -RID PhysicsServer3DSW::joint_create_slider(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { +void PhysicsServer3DSW::joint_make_slider(RID p_joint, RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { Body3DSW *body_A = body_owner.getornull(p_body_A); - ERR_FAIL_COND_V(!body_A, RID()); + ERR_FAIL_COND(!body_A); if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(), RID()); + ERR_FAIL_COND(!body_A->get_space()); p_body_B = body_A->get_space()->get_static_global_body(); } Body3DSW *body_B = body_owner.getornull(p_body_B); - ERR_FAIL_COND_V(!body_B, RID()); + ERR_FAIL_COND(!body_B); - ERR_FAIL_COND_V(body_A == body_B, RID()); + ERR_FAIL_COND(body_A == body_B); + + Joint3DSW *prev_joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); Joint3DSW *joint = memnew(SliderJoint3DSW(body_A, body_B, p_local_frame_A, p_local_frame_B)); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; + + joint->copy_settings_from(prev_joint); + joint_owner.replace(p_joint, joint); + memdelete(prev_joint); } void PhysicsServer3DSW::slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value) { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_SLIDER); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_SLIDER); SliderJoint3DSW *slider_joint = static_cast<SliderJoint3DSW *>(joint); slider_joint->set_param(p_param, p_value); } @@ -1112,35 +1145,39 @@ void PhysicsServer3DSW::slider_joint_set_param(RID p_joint, SliderJointParam p_p real_t PhysicsServer3DSW::slider_joint_get_param(RID p_joint, SliderJointParam p_param) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, 0); - ERR_FAIL_COND_V(joint->get_type() != JOINT_CONE_TWIST, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_CONE_TWIST, 0); SliderJoint3DSW *slider_joint = static_cast<SliderJoint3DSW *>(joint); return slider_joint->get_param(p_param); } -RID PhysicsServer3DSW::joint_create_cone_twist(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { +void PhysicsServer3DSW::joint_make_cone_twist(RID p_joint, RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { Body3DSW *body_A = body_owner.getornull(p_body_A); - ERR_FAIL_COND_V(!body_A, RID()); + ERR_FAIL_COND(!body_A); if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(), RID()); + ERR_FAIL_COND(!body_A->get_space()); p_body_B = body_A->get_space()->get_static_global_body(); } Body3DSW *body_B = body_owner.getornull(p_body_B); - ERR_FAIL_COND_V(!body_B, RID()); + ERR_FAIL_COND(!body_B); + + ERR_FAIL_COND(body_A == body_B); - ERR_FAIL_COND_V(body_A == body_B, RID()); + Joint3DSW *prev_joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); Joint3DSW *joint = memnew(ConeTwistJoint3DSW(body_A, body_B, p_local_frame_A, p_local_frame_B)); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; + + joint->copy_settings_from(prev_joint); + joint_owner.replace(p_joint, joint); + memdelete(prev_joint); } void PhysicsServer3DSW::cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t p_value) { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_CONE_TWIST); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_CONE_TWIST); ConeTwistJoint3DSW *cone_twist_joint = static_cast<ConeTwistJoint3DSW *>(joint); cone_twist_joint->set_param(p_param, p_value); } @@ -1148,43 +1185,47 @@ void PhysicsServer3DSW::cone_twist_joint_set_param(RID p_joint, ConeTwistJointPa real_t PhysicsServer3DSW::cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, 0); - ERR_FAIL_COND_V(joint->get_type() != JOINT_CONE_TWIST, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_CONE_TWIST, 0); ConeTwistJoint3DSW *cone_twist_joint = static_cast<ConeTwistJoint3DSW *>(joint); return cone_twist_joint->get_param(p_param); } -RID PhysicsServer3DSW::joint_create_generic_6dof(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { +void PhysicsServer3DSW::joint_make_generic_6dof(RID p_joint, RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) { Body3DSW *body_A = body_owner.getornull(p_body_A); - ERR_FAIL_COND_V(!body_A, RID()); + ERR_FAIL_COND(!body_A); if (!p_body_B.is_valid()) { - ERR_FAIL_COND_V(!body_A->get_space(), RID()); + ERR_FAIL_COND(!body_A->get_space()); p_body_B = body_A->get_space()->get_static_global_body(); } Body3DSW *body_B = body_owner.getornull(p_body_B); - ERR_FAIL_COND_V(!body_B, RID()); + ERR_FAIL_COND(!body_B); + + ERR_FAIL_COND(body_A == body_B); - ERR_FAIL_COND_V(body_A == body_B, RID()); + Joint3DSW *prev_joint = joint_owner.getornull(p_joint); + ERR_FAIL_COND(prev_joint == nullptr); Joint3DSW *joint = memnew(Generic6DOFJoint3DSW(body_A, body_B, p_local_frame_A, p_local_frame_B, true)); - RID rid = joint_owner.make_rid(joint); - joint->set_self(rid); - return rid; + + joint->copy_settings_from(prev_joint); + joint_owner.replace(p_joint, joint); + memdelete(prev_joint); } void PhysicsServer3DSW::generic_6dof_joint_set_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param, real_t p_value) { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_6DOF); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_6DOF); Generic6DOFJoint3DSW *generic_6dof_joint = static_cast<Generic6DOFJoint3DSW *>(joint); generic_6dof_joint->set_param(p_axis, p_param, p_value); } -real_t PhysicsServer3DSW::generic_6dof_joint_get_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param) { +real_t PhysicsServer3DSW::generic_6dof_joint_get_param(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisParam p_param) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, 0); - ERR_FAIL_COND_V(joint->get_type() != JOINT_6DOF, 0); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, 0); Generic6DOFJoint3DSW *generic_6dof_joint = static_cast<Generic6DOFJoint3DSW *>(joint); return generic_6dof_joint->get_param(p_axis, p_param); } @@ -1192,15 +1233,15 @@ real_t PhysicsServer3DSW::generic_6dof_joint_get_param(RID p_joint, Vector3::Axi void PhysicsServer3DSW::generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag, bool p_enable) { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND(!joint); - ERR_FAIL_COND(joint->get_type() != JOINT_6DOF); + ERR_FAIL_COND(joint->get_type() != JOINT_TYPE_6DOF); Generic6DOFJoint3DSW *generic_6dof_joint = static_cast<Generic6DOFJoint3DSW *>(joint); generic_6dof_joint->set_flag(p_axis, p_flag, p_enable); } -bool PhysicsServer3DSW::generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag) { +bool PhysicsServer3DSW::generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis p_axis, G6DOFJointAxisFlag p_flag) const { Joint3DSW *joint = joint_owner.getornull(p_joint); ERR_FAIL_COND_V(!joint, false); - ERR_FAIL_COND_V(joint->get_type() != JOINT_6DOF, false); + ERR_FAIL_COND_V(joint->get_type() != JOINT_TYPE_6DOF, false); Generic6DOFJoint3DSW *generic_6dof_joint = static_cast<Generic6DOFJoint3DSW *>(joint); return generic_6dof_joint->get_flag(p_axis, p_flag); } @@ -1317,6 +1358,10 @@ void PhysicsServer3DSW::step(real_t p_step) { #endif } +void PhysicsServer3DSW::sync() { + doing_sync = true; +}; + void PhysicsServer3DSW::flush_queries() { #ifndef _3D_DISABLED @@ -1370,6 +1415,10 @@ void PhysicsServer3DSW::flush_queries() { #endif }; +void PhysicsServer3DSW::end_sync() { + doing_sync = false; +}; + void PhysicsServer3DSW::finish() { memdelete(stepper); memdelete(direct_state); @@ -1431,14 +1480,15 @@ void PhysicsServer3DSW::_shape_col_cbk(const Vector3 &p_point_A, const Vector3 & } } -PhysicsServer3DSW *PhysicsServer3DSW::singleton = nullptr; -PhysicsServer3DSW::PhysicsServer3DSW() { - singleton = this; +PhysicsServer3DSW *PhysicsServer3DSW::singletonsw = nullptr; +PhysicsServer3DSW::PhysicsServer3DSW(bool p_using_threads) { + singletonsw = this; BroadPhase3DSW::create_func = BroadPhaseOctree::_create; island_count = 0; active_objects = 0; collision_pairs = 0; - + using_threads = p_using_threads; active = true; flushing_queries = false; + doing_sync = false; }; diff --git a/servers/physics_3d/physics_server_3d_sw.h b/servers/physics_3d/physics_server_3d_sw.h index c48db81d97..afda161fa8 100644 --- a/servers/physics_3d/physics_server_3d_sw.h +++ b/servers/physics_3d/physics_server_3d_sw.h @@ -50,6 +50,8 @@ class PhysicsServer3DSW : public PhysicsServer3D { int active_objects; int collision_pairs; + bool using_threads; + bool doing_sync; bool flushing_queries; Step3DSW *stepper; @@ -57,20 +59,20 @@ class PhysicsServer3DSW : public PhysicsServer3D { PhysicsDirectBodyState3DSW *direct_state; - mutable RID_PtrOwner<Shape3DSW> shape_owner; - mutable RID_PtrOwner<Space3DSW> space_owner; - mutable RID_PtrOwner<Area3DSW> area_owner; - mutable RID_PtrOwner<Body3DSW> body_owner; - mutable RID_PtrOwner<Joint3DSW> joint_owner; + mutable RID_PtrOwner<Shape3DSW, true> shape_owner; + mutable RID_PtrOwner<Space3DSW, true> space_owner; + mutable RID_PtrOwner<Area3DSW, true> area_owner; + mutable RID_PtrOwner<Body3DSW, true> body_owner; + mutable RID_PtrOwner<Joint3DSW, true> joint_owner; //void _clear_query(QuerySW *p_query); friend class CollisionObject3DSW; SelfList<CollisionObject3DSW>::List pending_shape_update_list; void _update_shapes(); -public: - static PhysicsServer3DSW *singleton; + static PhysicsServer3DSW *singletonsw; +public: struct CollCbkData { int max; int amount; @@ -79,7 +81,17 @@ public: static void _shape_col_cbk(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata); - virtual RID shape_create(ShapeType p_shape) override; + virtual RID plane_shape_create() override; + virtual RID ray_shape_create() override; + virtual RID sphere_shape_create() override; + virtual RID box_shape_create() override; + virtual RID capsule_shape_create() override; + virtual RID cylinder_shape_create() override; + virtual RID convex_polygon_shape_create() override; + virtual RID concave_polygon_shape_create() override; + virtual RID heightmap_shape_create() override; + virtual RID custom_shape_create() override; + virtual void shape_set_data(RID p_shape, const Variant &p_data) override; virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias) override; @@ -140,7 +152,6 @@ public: virtual Transform area_get_transform(RID p_area) const override; virtual void area_set_ray_pickable(RID p_area, bool p_enable) override; - virtual bool area_is_ray_pickable(RID p_area) const override; virtual void area_set_collision_mask(RID p_area, uint32_t p_mask) override; virtual void area_set_collision_layer(RID p_area, uint32_t p_layer) override; @@ -153,7 +164,7 @@ public: /* BODY API */ // create a body of a given type - virtual RID body_create(BodyMode p_mode = BODY_MODE_RIGID, bool p_init_sleeping = false) override; + virtual RID body_create() override; virtual void body_set_space(RID p_body, RID p_space) override; virtual RID body_get_space(RID p_body) const override; @@ -232,7 +243,6 @@ public: virtual void body_set_force_integration_callback(RID p_body, Object *p_receiver, const StringName &p_method, const Variant &p_udata = Variant()) override; virtual void body_set_ray_pickable(RID p_body, bool p_enable) override; - virtual bool body_is_ray_pickable(RID p_body) const override; virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) override; virtual int body_test_ray_separation(RID p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin = 0.001) override; @@ -242,7 +252,7 @@ public: /* SOFT BODY */ - virtual RID soft_body_create(bool p_init_sleeping = false) override { return RID(); } + virtual RID soft_body_create() override { return RID(); } virtual void soft_body_update_rendering_server(RID p_body, class SoftBodyRenderingServerHandler *p_rendering_server_handler) override {} @@ -266,49 +276,52 @@ public: virtual Vector3 soft_body_get_vertex_position(RID p_body, int vertex_index) const override { return Vector3(); } virtual void soft_body_set_ray_pickable(RID p_body, bool p_enable) override {} - virtual bool soft_body_is_ray_pickable(RID p_body) const override { return false; } virtual void soft_body_set_simulation_precision(RID p_body, int p_simulation_precision) override {} - virtual int soft_body_get_simulation_precision(RID p_body) override { return 0; } + virtual int soft_body_get_simulation_precision(RID p_body) const override { return 0; } virtual void soft_body_set_total_mass(RID p_body, real_t p_total_mass) override {} - virtual real_t soft_body_get_total_mass(RID p_body) override { return 0.; } + virtual real_t soft_body_get_total_mass(RID p_body) const override { return 0.; } virtual void soft_body_set_linear_stiffness(RID p_body, real_t p_stiffness) override {} - virtual real_t soft_body_get_linear_stiffness(RID p_body) override { return 0.; } + virtual real_t soft_body_get_linear_stiffness(RID p_body) const override { return 0.; } - virtual void soft_body_set_areaAngular_stiffness(RID p_body, real_t p_stiffness) override {} - virtual real_t soft_body_get_areaAngular_stiffness(RID p_body) override { return 0.; } + virtual void soft_body_set_angular_stiffness(RID p_body, real_t p_stiffness) override {} + virtual real_t soft_body_get_angular_stiffness(RID p_body) const override { return 0.; } virtual void soft_body_set_volume_stiffness(RID p_body, real_t p_stiffness) override {} - virtual real_t soft_body_get_volume_stiffness(RID p_body) override { return 0.; } + virtual real_t soft_body_get_volume_stiffness(RID p_body) const override { return 0.; } virtual void soft_body_set_pressure_coefficient(RID p_body, real_t p_pressure_coefficient) override {} - virtual real_t soft_body_get_pressure_coefficient(RID p_body) override { return 0.; } + virtual real_t soft_body_get_pressure_coefficient(RID p_body) const override { return 0.; } virtual void soft_body_set_pose_matching_coefficient(RID p_body, real_t p_pose_matching_coefficient) override {} - virtual real_t soft_body_get_pose_matching_coefficient(RID p_body) override { return 0.; } + virtual real_t soft_body_get_pose_matching_coefficient(RID p_body) const override { return 0.; } virtual void soft_body_set_damping_coefficient(RID p_body, real_t p_damping_coefficient) override {} - virtual real_t soft_body_get_damping_coefficient(RID p_body) override { return 0.; } + virtual real_t soft_body_get_damping_coefficient(RID p_body) const override { return 0.; } virtual void soft_body_set_drag_coefficient(RID p_body, real_t p_drag_coefficient) override {} - virtual real_t soft_body_get_drag_coefficient(RID p_body) override { return 0.; } + virtual real_t soft_body_get_drag_coefficient(RID p_body) const override { return 0.; } virtual void soft_body_set_mesh(RID p_body, const REF &p_mesh) override {} virtual void soft_body_move_point(RID p_body, int p_point_index, const Vector3 &p_global_position) override {} - virtual Vector3 soft_body_get_point_global_position(RID p_body, int p_point_index) override { return Vector3(); } + virtual Vector3 soft_body_get_point_global_position(RID p_body, int p_point_index) const override { return Vector3(); } virtual Vector3 soft_body_get_point_offset(RID p_body, int p_point_index) const override { return Vector3(); } virtual void soft_body_remove_all_pinned_points(RID p_body) override {} virtual void soft_body_pin_point(RID p_body, int p_point_index, bool p_pin) override {} - virtual bool soft_body_is_point_pinned(RID p_body, int p_point_index) override { return false; } + virtual bool soft_body_is_point_pinned(RID p_body, int p_point_index) const override { return false; } /* JOINT API */ - virtual RID joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) override; + virtual RID joint_create() override; + + virtual void joint_clear(RID p_joint) override; //resets type + + virtual void joint_make_pin(RID p_joint, RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) override; virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, real_t p_value) override; virtual real_t pin_joint_get_param(RID p_joint, PinJointParam p_param) const override; @@ -319,8 +332,8 @@ public: virtual void pin_joint_set_local_b(RID p_joint, const Vector3 &p_B) override; virtual Vector3 pin_joint_get_local_b(RID p_joint) const override; - virtual RID joint_create_hinge(RID p_body_A, const Transform &p_frame_A, RID p_body_B, const Transform &p_frame_B) override; - virtual RID joint_create_hinge_simple(RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B) override; + virtual void joint_make_hinge(RID p_joint, RID p_body_A, const Transform &p_frame_A, RID p_body_B, const Transform &p_frame_B) override; + virtual void joint_make_hinge_simple(RID p_joint, RID p_body_A, const Vector3 &p_pivot_A, const Vector3 &p_axis_A, RID p_body_B, const Vector3 &p_pivot_B, const Vector3 &p_axis_B) override; virtual void hinge_joint_set_param(RID p_joint, HingeJointParam p_param, real_t p_value) override; virtual real_t hinge_joint_get_param(RID p_joint, HingeJointParam p_param) const override; @@ -328,23 +341,23 @@ public: virtual void hinge_joint_set_flag(RID p_joint, HingeJointFlag p_flag, bool p_value) override; virtual bool hinge_joint_get_flag(RID p_joint, HingeJointFlag p_flag) const override; - virtual RID joint_create_slider(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) override; //reference frame is A + virtual void joint_make_slider(RID p_joint, RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) override; //reference frame is A virtual void slider_joint_set_param(RID p_joint, SliderJointParam p_param, real_t p_value) override; virtual real_t slider_joint_get_param(RID p_joint, SliderJointParam p_param) const override; - virtual RID joint_create_cone_twist(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) override; //reference frame is A + virtual void joint_make_cone_twist(RID p_joint, RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) override; //reference frame is A virtual void cone_twist_joint_set_param(RID p_joint, ConeTwistJointParam p_param, real_t p_value) override; virtual real_t cone_twist_joint_get_param(RID p_joint, ConeTwistJointParam p_param) const override; - virtual RID joint_create_generic_6dof(RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) override; //reference frame is A + virtual void joint_make_generic_6dof(RID p_joint, RID p_body_A, const Transform &p_local_frame_A, RID p_body_B, const Transform &p_local_frame_B) override; //reference frame is A virtual void generic_6dof_joint_set_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param, real_t p_value) override; - virtual real_t generic_6dof_joint_get_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param) override; + virtual real_t generic_6dof_joint_get_param(RID p_joint, Vector3::Axis, G6DOFJointAxisParam p_param) const override; virtual void generic_6dof_joint_set_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag, bool p_enable) override; - virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag) override; + virtual bool generic_6dof_joint_get_flag(RID p_joint, Vector3::Axis, G6DOFJointAxisFlag p_flag) const override; virtual JointType joint_get_type(RID p_joint) const override; @@ -361,14 +374,16 @@ public: virtual void set_active(bool p_active) override; virtual void init() override; virtual void step(real_t p_step) override; + virtual void sync() override; virtual void flush_queries() override; + virtual void end_sync() override; virtual void finish() override; virtual bool is_flushing_queries() const override { return flushing_queries; } int get_process_info(ProcessInfo p_info) override; - PhysicsServer3DSW(); + PhysicsServer3DSW(bool p_using_threads = false); ~PhysicsServer3DSW() {} }; diff --git a/servers/physics_3d/physics_server_3d_wrap_mt.cpp b/servers/physics_3d/physics_server_3d_wrap_mt.cpp new file mode 100644 index 0000000000..f73f67a756 --- /dev/null +++ b/servers/physics_3d/physics_server_3d_wrap_mt.cpp @@ -0,0 +1,140 @@ +/*************************************************************************/ +/* physics_server_3d_wrap_mt.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "physics_server_3d_wrap_mt.h" + +#include "core/os/os.h" + +void PhysicsServer3DWrapMT::thread_exit() { + exit = true; +} + +void PhysicsServer3DWrapMT::thread_step(real_t p_delta) { + physics_3d_server->step(p_delta); + step_sem.post(); +} + +void PhysicsServer3DWrapMT::_thread_callback(void *_instance) { + PhysicsServer3DWrapMT *vsmt = reinterpret_cast<PhysicsServer3DWrapMT *>(_instance); + + vsmt->thread_loop(); +} + +void PhysicsServer3DWrapMT::thread_loop() { + server_thread = Thread::get_caller_id(); + + physics_3d_server->init(); + + exit = false; + step_thread_up = true; + while (!exit) { + // flush commands one by one, until exit is requested + command_queue.wait_and_flush_one(); + } + + command_queue.flush_all(); // flush all + + physics_3d_server->finish(); +} + +/* EVENT QUEUING */ + +void PhysicsServer3DWrapMT::step(real_t p_step) { + if (create_thread) { + command_queue.push(this, &PhysicsServer3DWrapMT::thread_step, p_step); + } else { + command_queue.flush_all(); //flush all pending from other threads + physics_3d_server->step(p_step); + } +} + +void PhysicsServer3DWrapMT::sync() { + if (create_thread) { + if (first_frame) { + first_frame = false; + } else { + step_sem.wait(); //must not wait if a step was not issued + } + } + physics_3d_server->sync(); +} + +void PhysicsServer3DWrapMT::flush_queries() { + physics_3d_server->flush_queries(); +} + +void PhysicsServer3DWrapMT::end_sync() { + physics_3d_server->end_sync(); +} + +void PhysicsServer3DWrapMT::init() { + if (create_thread) { + //OS::get_singleton()->release_rendering_thread(); + thread.start(_thread_callback, this); + while (!step_thread_up) { + OS::get_singleton()->delay_usec(1000); + } + } else { + physics_3d_server->init(); + } +} + +void PhysicsServer3DWrapMT::finish() { + if (thread.is_started()) { + command_queue.push(this, &PhysicsServer3DWrapMT::thread_exit); + thread.wait_to_finish(); + } else { + physics_3d_server->finish(); + } +} + +PhysicsServer3DWrapMT::PhysicsServer3DWrapMT(PhysicsServer3D *p_contained, bool p_create_thread) : + command_queue(p_create_thread) { + physics_3d_server = p_contained; + create_thread = p_create_thread; + step_pending = 0; + step_thread_up = false; + + pool_max_size = GLOBAL_GET("memory/limits/multithreaded_server/rid_pool_prealloc"); + + if (!p_create_thread) { + server_thread = Thread::get_caller_id(); + } else { + server_thread = 0; + } + + main_thread = Thread::get_caller_id(); + first_frame = true; +} + +PhysicsServer3DWrapMT::~PhysicsServer3DWrapMT() { + memdelete(physics_3d_server); + //finish(); +} diff --git a/servers/physics_3d/physics_server_3d_wrap_mt.h b/servers/physics_3d/physics_server_3d_wrap_mt.h new file mode 100644 index 0000000000..f60e1332d5 --- /dev/null +++ b/servers/physics_3d/physics_server_3d_wrap_mt.h @@ -0,0 +1,422 @@ +/*************************************************************************/ +/* physics_server_3d_wrap_mt.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef PHYSICS3DSERVERWRAPMT_H +#define PHYSICS3DSERVERWRAPMT_H + +#include "core/config/project_settings.h" +#include "core/os/thread.h" +#include "core/templates/command_queue_mt.h" +#include "servers/physics_server_3d.h" + +#ifdef DEBUG_SYNC +#define SYNC_DEBUG print_line("sync on: " + String(__FUNCTION__)); +#else +#define SYNC_DEBUG +#endif + +class PhysicsServer3DWrapMT : public PhysicsServer3D { + mutable PhysicsServer3D *physics_3d_server; + + mutable CommandQueueMT command_queue; + + static void _thread_callback(void *_instance); + void thread_loop(); + + Thread::ID server_thread; + Thread::ID main_thread; + volatile bool exit = false; + Thread thread; + volatile bool step_thread_up = false; + bool create_thread = false; + + Semaphore step_sem; + int step_pending; + void thread_step(real_t p_delta); + void thread_flush(); + + void thread_exit(); + + bool first_frame = true; + + Mutex alloc_mutex; + int pool_max_size = 0; + +public: +#define ServerName PhysicsServer3D +#define ServerNameWrapMT PhysicsServer3DWrapMT +#define server_name physics_3d_server +#define WRITE_ACTION + +#include "servers/server_wrap_mt_common.h" + + //FUNC1RID(shape,ShapeType); todo fix + FUNCRID(plane_shape) + FUNCRID(ray_shape) + FUNCRID(sphere_shape) + FUNCRID(box_shape) + FUNCRID(capsule_shape) + FUNCRID(cylinder_shape) + FUNCRID(convex_polygon_shape) + FUNCRID(concave_polygon_shape) + FUNCRID(heightmap_shape) + FUNCRID(custom_shape) + + FUNC2(shape_set_data, RID, const Variant &); + FUNC2(shape_set_custom_solver_bias, RID, real_t); + + FUNC2(shape_set_margin, RID, real_t) + FUNC1RC(real_t, shape_get_margin, RID) + + FUNC1RC(ShapeType, shape_get_type, RID); + FUNC1RC(Variant, shape_get_data, RID); + FUNC1RC(real_t, shape_get_custom_solver_bias, RID); +#if 0 + //these work well, but should be used from the main thread only + bool shape_collide(RID p_shape_A, const Transform &p_xform_A, const Vector3 &p_motion_A, RID p_shape_B, const Transform &p_xform_B, const Vector3 &p_motion_B, Vector3 *r_results, int p_result_max, int &r_result_count) { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false); + return physics_3d_server->shape_collide(p_shape_A, p_xform_A, p_motion_A, p_shape_B, p_xform_B, p_motion_B, r_results, p_result_max, r_result_count); + } +#endif + /* SPACE API */ + + FUNCRID(space); + FUNC2(space_set_active, RID, bool); + FUNC1RC(bool, space_is_active, RID); + + FUNC3(space_set_param, RID, SpaceParameter, real_t); + FUNC2RC(real_t, space_get_param, RID, SpaceParameter); + + // this function only works on physics process, errors and returns null otherwise + PhysicsDirectSpaceState3D *space_get_direct_state(RID p_space) override { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), nullptr); + return physics_3d_server->space_get_direct_state(p_space); + } + + FUNC2(space_set_debug_contacts, RID, int); + virtual Vector<Vector3> space_get_contacts(RID p_space) const override { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), Vector<Vector3>()); + return physics_3d_server->space_get_contacts(p_space); + } + + virtual int space_get_contact_count(RID p_space) const override { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), 0); + return physics_3d_server->space_get_contact_count(p_space); + } + + /* AREA API */ + + //FUNC0RID(area); + FUNCRID(area); + + FUNC2(area_set_space, RID, RID); + FUNC1RC(RID, area_get_space, RID); + + FUNC2(area_set_space_override_mode, RID, AreaSpaceOverrideMode); + FUNC1RC(AreaSpaceOverrideMode, area_get_space_override_mode, RID); + + FUNC4(area_add_shape, RID, RID, const Transform &, bool); + FUNC3(area_set_shape, RID, int, RID); + FUNC3(area_set_shape_transform, RID, int, const Transform &); + FUNC3(area_set_shape_disabled, RID, int, bool); + + FUNC1RC(int, area_get_shape_count, RID); + FUNC2RC(RID, area_get_shape, RID, int); + FUNC2RC(Transform, area_get_shape_transform, RID, int); + FUNC2(area_remove_shape, RID, int); + FUNC1(area_clear_shapes, RID); + + FUNC2(area_attach_object_instance_id, RID, ObjectID); + FUNC1RC(ObjectID, area_get_object_instance_id, RID); + + FUNC3(area_set_param, RID, AreaParameter, const Variant &); + FUNC2(area_set_transform, RID, const Transform &); + + FUNC2RC(Variant, area_get_param, RID, AreaParameter); + FUNC1RC(Transform, area_get_transform, RID); + + FUNC2(area_set_collision_mask, RID, uint32_t); + FUNC2(area_set_collision_layer, RID, uint32_t); + + FUNC2(area_set_monitorable, RID, bool); + FUNC2(area_set_ray_pickable, RID, bool); + + FUNC3(area_set_monitor_callback, RID, Object *, const StringName &); + FUNC3(area_set_area_monitor_callback, RID, Object *, const StringName &); + + /* BODY API */ + + //FUNC2RID(body,BodyMode,bool); + FUNCRID(body) + + FUNC2(body_set_space, RID, RID); + FUNC1RC(RID, body_get_space, RID); + + FUNC2(body_set_mode, RID, BodyMode); + FUNC1RC(BodyMode, body_get_mode, RID); + + FUNC4(body_add_shape, RID, RID, const Transform &, bool); + FUNC3(body_set_shape, RID, int, RID); + FUNC3(body_set_shape_transform, RID, int, const Transform &); + + FUNC1RC(int, body_get_shape_count, RID); + FUNC2RC(Transform, body_get_shape_transform, RID, int); + FUNC2RC(RID, body_get_shape, RID, int); + + FUNC3(body_set_shape_disabled, RID, int, bool); + + FUNC2(body_remove_shape, RID, int); + FUNC1(body_clear_shapes, RID); + + FUNC2(body_attach_object_instance_id, RID, ObjectID); + FUNC1RC(ObjectID, body_get_object_instance_id, RID); + + FUNC2(body_set_enable_continuous_collision_detection, RID, bool); + FUNC1RC(bool, body_is_continuous_collision_detection_enabled, RID); + + FUNC2(body_set_collision_layer, RID, uint32_t); + FUNC1RC(uint32_t, body_get_collision_layer, RID); + + FUNC2(body_set_collision_mask, RID, uint32_t); + FUNC1RC(uint32_t, body_get_collision_mask, RID); + + FUNC2(body_set_user_flags, RID, uint32_t); + FUNC1RC(uint32_t, body_get_user_flags, RID); + + FUNC3(body_set_param, RID, BodyParameter, real_t); + FUNC2RC(real_t, body_get_param, RID, BodyParameter); + + FUNC2(body_set_kinematic_safe_margin, RID, real_t); + FUNC1RC(real_t, body_get_kinematic_safe_margin, RID); + + FUNC3(body_set_state, RID, BodyState, const Variant &); + FUNC2RC(Variant, body_get_state, RID, BodyState); + + FUNC2(body_set_applied_force, RID, const Vector3 &); + FUNC1RC(Vector3, body_get_applied_force, RID); + + FUNC2(body_set_applied_torque, RID, const Vector3 &); + FUNC1RC(Vector3, body_get_applied_torque, RID); + + FUNC2(body_add_central_force, RID, const Vector3 &); + FUNC3(body_add_force, RID, const Vector3 &, const Vector3 &); + FUNC2(body_add_torque, RID, const Vector3 &); + FUNC2(body_apply_torque_impulse, RID, const Vector3 &); + FUNC2(body_apply_central_impulse, RID, const Vector3 &); + FUNC3(body_apply_impulse, RID, const Vector3 &, const Vector3 &); + FUNC2(body_set_axis_velocity, RID, const Vector3 &); + + FUNC3(body_set_axis_lock, RID, BodyAxis, bool); + FUNC2RC(bool, body_is_axis_locked, RID, BodyAxis); + + FUNC2(body_add_collision_exception, RID, RID); + FUNC2(body_remove_collision_exception, RID, RID); + FUNC2S(body_get_collision_exceptions, RID, List<RID> *); + + FUNC2(body_set_max_contacts_reported, RID, int); + FUNC1RC(int, body_get_max_contacts_reported, RID); + + FUNC2(body_set_contacts_reported_depth_threshold, RID, real_t); + FUNC1RC(real_t, body_get_contacts_reported_depth_threshold, RID); + + FUNC2(body_set_omit_force_integration, RID, bool); + FUNC1RC(bool, body_is_omitting_force_integration, RID); + + FUNC4(body_set_force_integration_callback, RID, Object *, const StringName &, const Variant &); + + FUNC2(body_set_ray_pickable, RID, bool); + + bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result = nullptr, bool p_exclude_raycast_shapes = true) override { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false); + return physics_3d_server->body_test_motion(p_body, p_from, p_motion, p_infinite_inertia, r_result, p_exclude_raycast_shapes); + } + + int body_test_ray_separation(RID p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, real_t p_margin = 0.001) override { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), false); + return physics_3d_server->body_test_ray_separation(p_body, p_transform, p_infinite_inertia, r_recover_motion, r_results, p_result_max, p_margin); + } + + // this function only works on physics process, errors and returns null otherwise + PhysicsDirectBodyState3D *body_get_direct_state(RID p_body) override { + ERR_FAIL_COND_V(main_thread != Thread::get_caller_id(), nullptr); + return physics_3d_server->body_get_direct_state(p_body); + } + + /* SOFT BODY API */ + + FUNCRID(soft_body) + + FUNC2(soft_body_update_rendering_server, RID, class SoftBodyRenderingServerHandler *) + + FUNC2(soft_body_set_space, RID, RID) + FUNC1RC(RID, soft_body_get_space, RID) + + FUNC2(soft_body_set_ray_pickable, RID, bool); + + FUNC2(soft_body_set_collision_layer, RID, uint32_t) + FUNC1RC(uint32_t, soft_body_get_collision_layer, RID) + + FUNC2(soft_body_set_collision_mask, RID, uint32_t) + FUNC1RC(uint32_t, soft_body_get_collision_mask, RID) + + FUNC2(soft_body_add_collision_exception, RID, RID) + FUNC2(soft_body_remove_collision_exception, RID, RID) + FUNC2S(soft_body_get_collision_exceptions, RID, List<RID> *) + + FUNC3(soft_body_set_state, RID, BodyState, const Variant &); + FUNC2RC(Variant, soft_body_get_state, RID, BodyState); + + FUNC2(soft_body_set_transform, RID, const Transform &); + FUNC2RC(Vector3, soft_body_get_vertex_position, RID, int); + + FUNC2(soft_body_set_simulation_precision, RID, int); + FUNC1RC(int, soft_body_get_simulation_precision, RID); + + FUNC2(soft_body_set_total_mass, RID, real_t); + FUNC1RC(real_t, soft_body_get_total_mass, RID); + + FUNC2(soft_body_set_linear_stiffness, RID, real_t); + FUNC1RC(real_t, soft_body_get_linear_stiffness, RID); + + FUNC2(soft_body_set_angular_stiffness, RID, real_t); + FUNC1RC(real_t, soft_body_get_angular_stiffness, RID); + + FUNC2(soft_body_set_volume_stiffness, RID, real_t); + FUNC1RC(real_t, soft_body_get_volume_stiffness, RID); + + FUNC2(soft_body_set_pressure_coefficient, RID, real_t); + FUNC1RC(real_t, soft_body_get_pressure_coefficient, RID); + + FUNC2(soft_body_set_pose_matching_coefficient, RID, real_t); + FUNC1RC(real_t, soft_body_get_pose_matching_coefficient, RID); + + FUNC2(soft_body_set_damping_coefficient, RID, real_t); + FUNC1RC(real_t, soft_body_get_damping_coefficient, RID); + + FUNC2(soft_body_set_drag_coefficient, RID, real_t); + FUNC1RC(real_t, soft_body_get_drag_coefficient, RID); + + FUNC2(soft_body_set_mesh, RID, const REF &); + + FUNC3(soft_body_move_point, RID, int, const Vector3 &); + FUNC2RC(Vector3, soft_body_get_point_global_position, RID, int); + FUNC2RC(Vector3, soft_body_get_point_offset, RID, int); + + FUNC1(soft_body_remove_all_pinned_points, RID); + FUNC3(soft_body_pin_point, RID, int, bool); + FUNC2RC(bool, soft_body_is_point_pinned, RID, int); + + /* JOINT API */ + + FUNCRID(joint) + + FUNC1(joint_clear, RID) + + FUNC5(joint_make_pin, RID, RID, const Vector3 &, RID, const Vector3 &) + + FUNC3(pin_joint_set_param, RID, PinJointParam, real_t) + FUNC2RC(real_t, pin_joint_get_param, RID, PinJointParam) + + FUNC2(pin_joint_set_local_a, RID, const Vector3 &) + FUNC1RC(Vector3, pin_joint_get_local_a, RID) + + FUNC2(pin_joint_set_local_b, RID, const Vector3 &) + FUNC1RC(Vector3, pin_joint_get_local_b, RID) + + FUNC5(joint_make_hinge, RID, RID, const Transform &, RID, const Transform &) + FUNC7(joint_make_hinge_simple, RID, RID, const Vector3 &, const Vector3 &, RID, const Vector3 &, const Vector3 &) + + FUNC3(hinge_joint_set_param, RID, HingeJointParam, real_t) + FUNC2RC(real_t, hinge_joint_get_param, RID, HingeJointParam) + + FUNC3(hinge_joint_set_flag, RID, HingeJointFlag, bool) + FUNC2RC(bool, hinge_joint_get_flag, RID, HingeJointFlag) + + FUNC5(joint_make_slider, RID, RID, const Transform &, RID, const Transform &) + + FUNC3(slider_joint_set_param, RID, SliderJointParam, real_t) + FUNC2RC(real_t, slider_joint_get_param, RID, SliderJointParam) + + FUNC5(joint_make_cone_twist, RID, RID, const Transform &, RID, const Transform &) + + FUNC3(cone_twist_joint_set_param, RID, ConeTwistJointParam, real_t) + FUNC2RC(real_t, cone_twist_joint_get_param, RID, ConeTwistJointParam) + + FUNC5(joint_make_generic_6dof, RID, RID, const Transform &, RID, const Transform &) + + FUNC4(generic_6dof_joint_set_param, RID, Vector3::Axis, G6DOFJointAxisParam, real_t) + FUNC3RC(real_t, generic_6dof_joint_get_param, RID, Vector3::Axis, G6DOFJointAxisParam) + + FUNC4(generic_6dof_joint_set_flag, RID, Vector3::Axis, G6DOFJointAxisFlag, bool) + FUNC3RC(bool, generic_6dof_joint_get_flag, RID, Vector3::Axis, G6DOFJointAxisFlag) + + FUNC1RC(JointType, joint_get_type, RID); + + FUNC2(joint_set_solver_priority, RID, int); + FUNC1RC(int, joint_get_solver_priority, RID); + + FUNC2(joint_disable_collisions_between_bodies, RID, const bool); + FUNC1RC(bool, joint_is_disabled_collisions_between_bodies, RID); + + /* MISC */ + + FUNC1(free, RID); + FUNC1(set_active, bool); + + virtual void init() override; + virtual void step(real_t p_step) override; + virtual void sync() override; + virtual void end_sync() override; + virtual void flush_queries() override; + virtual void finish() override; + + virtual bool is_flushing_queries() const override { + return physics_3d_server->is_flushing_queries(); + } + + int get_process_info(ProcessInfo p_info) override { + return physics_3d_server->get_process_info(p_info); + } + + PhysicsServer3DWrapMT(PhysicsServer3D *p_contained, bool p_create_thread); + ~PhysicsServer3DWrapMT(); + +#undef ServerNameWrapMT +#undef ServerName +#undef server_name +#undef WRITE_ACTION +}; + +#ifdef DEBUG_SYNC +#undef DEBUG_SYNC +#endif +#undef SYNC_DEBUG + +#endif // PHYSICS3DSERVERWRAPMT_H diff --git a/servers/physics_3d/shape_3d_sw.cpp b/servers/physics_3d/shape_3d_sw.cpp index 9c37060bea..5bac4f19b9 100644 --- a/servers/physics_3d/shape_3d_sw.cpp +++ b/servers/physics_3d/shape_3d_sw.cpp @@ -34,10 +34,12 @@ #include "core/math/quick_hull.h" #include "core/templates/sort_array.h" -#define _POINT_SNAP 0.001953125 #define _EDGE_IS_VALID_SUPPORT_THRESHOLD 0.0002 #define _FACE_IS_VALID_SUPPORT_THRESHOLD 0.9998 +#define _CYLINDER_EDGE_IS_VALID_SUPPORT_THRESHOLD 0.002 +#define _CYLINDER_FACE_IS_VALID_SUPPORT_THRESHOLD 0.999 + void Shape3DSW::configure(const AABB &p_aabb) { aabb = p_aabb; configured = true; @@ -50,7 +52,8 @@ void Shape3DSW::configure(const AABB &p_aabb) { Vector3 Shape3DSW::get_support(const Vector3 &p_normal) const { Vector3 res; int amnt; - get_supports(p_normal, 1, &res, amnt); + FeatureType type; + get_supports(p_normal, 1, &res, amnt, type); return res; } @@ -167,16 +170,19 @@ Vector3 RayShape3DSW::get_support(const Vector3 &p_normal) const { } } -void RayShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { +void RayShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { if (Math::abs(p_normal.z) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { r_amount = 2; + r_type = FEATURE_EDGE; r_supports[0] = Vector3(0, 0, 0); r_supports[1] = Vector3(0, 0, length); } else if (p_normal.z > 0) { r_amount = 1; + r_type = FEATURE_POINT; *r_supports = Vector3(0, 0, length); } else { r_amount = 1; + r_type = FEATURE_POINT; *r_supports = Vector3(0, 0, 0); } } @@ -246,9 +252,10 @@ Vector3 SphereShape3DSW::get_support(const Vector3 &p_normal) const { return p_normal * radius; } -void SphereShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { +void SphereShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { *r_supports = p_normal * radius; r_amount = 1; + r_type = FEATURE_POINT; } bool SphereShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { @@ -312,7 +319,7 @@ Vector3 BoxShape3DSW::get_support(const Vector3 &p_normal) const { return point; } -void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { +void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { static const int next[3] = { 1, 2, 0 }; static const int next2[3] = { 2, 0, 1 }; @@ -325,6 +332,7 @@ void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_s bool neg = dot < 0; r_amount = 4; + r_type = FEATURE_FACE; Vector3 point; point[i] = half_extents[i]; @@ -362,6 +370,7 @@ void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_s if (Math::abs(p_normal.dot(axis)) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { r_amount = 2; + r_type = FEATURE_EDGE; int i_n = next[i]; int i_n2 = next2[i]; @@ -389,6 +398,7 @@ void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_s (p_normal.z < 0) ? -half_extents.z : half_extents.z); r_amount = 1; + r_type = FEATURE_POINT; r_supports[0] = point; } @@ -500,7 +510,7 @@ Vector3 CapsuleShape3DSW::get_support(const Vector3 &p_normal) const { return n; } -void CapsuleShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { +void CapsuleShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { Vector3 n = p_normal; real_t d = n.z; @@ -512,6 +522,7 @@ void CapsuleShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 n *= radius; r_amount = 2; + r_type = FEATURE_EDGE; r_supports[0] = n; r_supports[0].z += height * 0.5; r_supports[1] = n; @@ -523,6 +534,7 @@ void CapsuleShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 n *= radius; n.z += h * 0.5; r_amount = 1; + r_type = FEATURE_POINT; *r_supports = n; } } @@ -642,6 +654,186 @@ CapsuleShape3DSW::CapsuleShape3DSW() { height = radius = 0; } +/********** CYLINDER *************/ + +void CylinderShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { + Vector3 cylinder_axis = p_transform.basis.get_axis(1).normalized(); + real_t axis_dot = cylinder_axis.dot(p_normal); + + Vector3 local_normal = p_transform.basis.xform_inv(p_normal); + real_t scale = local_normal.length(); + real_t scaled_radius = radius * scale; + real_t scaled_height = height * scale; + + real_t length; + if (Math::abs(axis_dot) > 1.0) { + length = scaled_height * 0.5; + } else { + length = Math::abs(axis_dot * scaled_height * 0.5) + scaled_radius * Math::sqrt(1.0 - axis_dot * axis_dot); + } + + real_t distance = p_normal.dot(p_transform.origin); + + r_min = distance - length; + r_max = distance + length; +} + +Vector3 CylinderShape3DSW::get_support(const Vector3 &p_normal) const { + Vector3 n = p_normal; + real_t h = (n.y > 0) ? height : -height; + real_t s = Math::sqrt(n.x * n.x + n.z * n.z); + if (Math::is_zero_approx(s)) { + n.x = radius; + n.y = h * 0.5; + n.z = 0.0; + } else { + real_t d = radius / s; + n.x = n.x * d; + n.y = h * 0.5; + n.z = n.z * d; + } + + return n; +} + +void CylinderShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { + real_t d = p_normal.y; + if (Math::abs(d) > _CYLINDER_FACE_IS_VALID_SUPPORT_THRESHOLD) { + real_t h = (d > 0) ? height : -height; + + Vector3 n = p_normal; + n.x = 0.0; + n.z = 0.0; + n.y = h * 0.5; + + r_amount = 3; + r_type = FEATURE_CIRCLE; + r_supports[0] = n; + r_supports[1] = n; + r_supports[1].x += radius; + r_supports[2] = n; + r_supports[2].z += radius; + } else if (Math::abs(d) < _CYLINDER_EDGE_IS_VALID_SUPPORT_THRESHOLD) { + // make it flat + Vector3 n = p_normal; + n.y = 0.0; + n.normalize(); + n *= radius; + + r_amount = 2; + r_type = FEATURE_EDGE; + r_supports[0] = n; + r_supports[0].y += height * 0.5; + r_supports[1] = n; + r_supports[1].y -= height * 0.5; + } else { + r_amount = 1; + r_type = FEATURE_POINT; + r_supports[0] = get_support(p_normal); + return; + + Vector3 n = p_normal; + real_t h = n.y * Math::sqrt(0.25 * height * height + radius * radius); + if (Math::abs(h) > 1.0) { + // Top or bottom surface. + n.y = (n.y > 0.0) ? height * 0.5 : -height * 0.5; + } else { + // Lateral surface. + n.y = height * 0.5 * h; + } + + real_t s = Math::sqrt(n.x * n.x + n.z * n.z); + if (Math::is_zero_approx(s)) { + n.x = 0.0; + n.z = 0.0; + } else { + real_t scaled_radius = radius / s; + n.x = n.x * scaled_radius; + n.z = n.z * scaled_radius; + } + + r_supports[0] = n; + } +} + +bool CylinderShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { + return Geometry3D::segment_intersects_cylinder(p_begin, p_end, height, radius, &r_result, &r_normal, 1); +} + +bool CylinderShape3DSW::intersect_point(const Vector3 &p_point) const { + if (Math::abs(p_point.y) < height * 0.5) { + return Vector3(p_point.x, 0, p_point.z).length() < radius; + } + return false; +} + +Vector3 CylinderShape3DSW::get_closest_point_to(const Vector3 &p_point) const { + if (Math::absf(p_point.y) > height * 0.5) { + // Project point to top disk. + real_t dir = p_point.y > 0.0 ? 1.0 : -1.0; + Vector3 circle_pos(0.0, dir * height * 0.5, 0.0); + Plane circle_plane(circle_pos, Vector3(0.0, dir, 0.0)); + Vector3 proj_point = circle_plane.project(p_point); + + // Clip position. + Vector3 delta_point_1 = proj_point - circle_pos; + real_t dist_point_1 = delta_point_1.length_squared(); + if (!Math::is_zero_approx(dist_point_1)) { + dist_point_1 = Math::sqrt(dist_point_1); + proj_point = circle_pos + delta_point_1 * MIN(dist_point_1, radius) / dist_point_1; + } + + return proj_point; + } else { + Vector3 s[2] = { + Vector3(0, -height * 0.5, 0), + Vector3(0, height * 0.5, 0), + }; + + Vector3 p = Geometry3D::get_closest_point_to_segment(p_point, s); + + if (p.distance_to(p_point) < radius) { + return p_point; + } + + return p + (p_point - p).normalized() * radius; + } +} + +Vector3 CylinderShape3DSW::get_moment_of_inertia(real_t p_mass) const { + // use bad AABB approximation + Vector3 extents = get_aabb().size * 0.5; + + return Vector3( + (p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z), + (p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z), + (p_mass / 3.0) * (extents.y * extents.y + extents.y * extents.y)); +} + +void CylinderShape3DSW::_setup(real_t p_height, real_t p_radius) { + height = p_height; + radius = p_radius; + configure(AABB(Vector3(-radius, -height * 0.5, -radius), Vector3(radius * 2.0, height, radius * 2.0))); +} + +void CylinderShape3DSW::set_data(const Variant &p_data) { + Dictionary d = p_data; + ERR_FAIL_COND(!d.has("radius")); + ERR_FAIL_COND(!d.has("height")); + _setup(d["height"], d["radius"]); +} + +Variant CylinderShape3DSW::get_data() const { + Dictionary d; + d["radius"] = radius; + d["height"] = height; + return d; +} + +CylinderShape3DSW::CylinderShape3DSW() { + height = radius = 0; +} + /********** CONVEX POLYGON *************/ void ConvexPolygonShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const { @@ -689,7 +881,7 @@ Vector3 ConvexPolygonShape3DSW::get_support(const Vector3 &p_normal) const { return vrts[vert_support_idx]; } -void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { +void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { const Geometry3D::MeshData::Face *faces = mesh.faces.ptr(); int fc = mesh.faces.size(); @@ -734,6 +926,7 @@ void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Ve r_supports[j] = vertices[ind[j]]; } r_amount = m; + r_type = FEATURE_FACE; return; } } @@ -743,6 +936,7 @@ void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Ve dot = ABS(dot); if (dot < _EDGE_IS_VALID_SUPPORT_THRESHOLD && (edges[i].a == vtx || edges[i].b == vtx)) { r_amount = 2; + r_type = FEATURE_EDGE; r_supports[0] = vertices[edges[i].a]; r_supports[1] = vertices[edges[i].b]; return; @@ -751,6 +945,7 @@ void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Ve r_supports[0] = vertices[vtx]; r_amount = 1; + r_type = FEATURE_POINT; } bool ConvexPolygonShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { @@ -935,12 +1130,13 @@ Vector3 FaceShape3DSW::get_support(const Vector3 &p_normal) const { return vertex[vert_support_idx]; } -void FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { +void FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { Vector3 n = p_normal; /** TEST FACE AS SUPPORT **/ if (normal.dot(n) > _FACE_IS_VALID_SUPPORT_THRESHOLD) { r_amount = 3; + r_type = FEATURE_FACE; for (int i = 0; i < 3; i++) { r_supports[i] = vertex[i]; } @@ -974,6 +1170,7 @@ void FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_ dot = ABS(dot); if (dot < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { r_amount = 2; + r_type = FEATURE_EDGE; r_supports[0] = vertex[i]; r_supports[1] = vertex[nx]; return; @@ -981,6 +1178,7 @@ void FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_ } r_amount = 1; + r_type = FEATURE_POINT; r_supports[0] = vertex[vert_support_idx]; } diff --git a/servers/physics_3d/shape_3d_sw.h b/servers/physics_3d/shape_3d_sw.h index 851c0d9443..cafe978abb 100644 --- a/servers/physics_3d/shape_3d_sw.h +++ b/servers/physics_3d/shape_3d_sw.h @@ -67,8 +67,11 @@ protected: void configure(const AABB &p_aabb); public: - enum { - MAX_SUPPORTS = 8 + enum FeatureType { + FEATURE_POINT, + FEATURE_EDGE, + FEATURE_FACE, + FEATURE_CIRCLE, }; virtual real_t get_area() const { return aabb.get_area(); } @@ -85,7 +88,7 @@ public: virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const = 0; virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const = 0; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const = 0; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const = 0; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const = 0; virtual bool intersect_point(const Vector3 &p_point) const = 0; @@ -110,7 +113,7 @@ class ConcaveShape3DSW : public Shape3DSW { public: virtual bool is_concave() const { return true; } typedef void (*Callback)(void *p_userdata, Shape3DSW *p_convex); - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { r_amount = 0; } + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { r_amount = 0; } virtual void cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const = 0; @@ -129,7 +132,7 @@ public: virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_PLANE; } virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { r_amount = 0; } + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { r_amount = 0; } virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; virtual bool intersect_point(const Vector3 &p_point) const; @@ -156,7 +159,7 @@ public: virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_RAY; } virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; virtual bool intersect_point(const Vector3 &p_point) const; @@ -184,7 +187,7 @@ public: virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; virtual bool intersect_point(const Vector3 &p_point) const; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; @@ -209,7 +212,7 @@ public: virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; virtual bool intersect_point(const Vector3 &p_point) const; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; @@ -238,7 +241,7 @@ public: virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; virtual bool intersect_point(const Vector3 &p_point) const; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; @@ -251,6 +254,35 @@ public: CapsuleShape3DSW(); }; +class CylinderShape3DSW : public Shape3DSW { + real_t height; + real_t radius; + + void _setup(real_t p_height, real_t p_radius); + +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 PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CYLINDER; } + + virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; + virtual Vector3 get_support(const Vector3 &p_normal) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; + virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; + virtual bool intersect_point(const Vector3 &p_point) const; + virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; + + virtual Vector3 get_moment_of_inertia(real_t p_mass) const; + + virtual void set_data(const Variant &p_data); + virtual Variant get_data() const; + + CylinderShape3DSW(); +}; + struct ConvexPolygonShape3DSW : public Shape3DSW { Geometry3D::MeshData mesh; @@ -263,7 +295,7 @@ public: virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; virtual Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; virtual bool intersect_point(const Vector3 &p_point) const; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; @@ -399,7 +431,7 @@ struct FaceShape3DSW : public Shape3DSW { void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const; Vector3 get_support(const Vector3 &p_normal) const; - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const; + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const; bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const; virtual bool intersect_point(const Vector3 &p_point) const; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const; @@ -437,7 +469,7 @@ struct MotionShape3DSW : public Shape3DSW { } return support; } - virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { r_amount = 0; } + virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { r_amount = 0; } bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { return false; } virtual bool intersect_point(const Vector3 &p_point) const { return false; } virtual Vector3 get_closest_point_to(const Vector3 &p_point) const { return p_point; } diff --git a/servers/physics_3d/space_3d_sw.cpp b/servers/physics_3d/space_3d_sw.cpp index 43cc032120..40537c7001 100644 --- a/servers/physics_3d/space_3d_sw.cpp +++ b/servers/physics_3d/space_3d_sw.cpp @@ -181,7 +181,7 @@ int PhysicsDirectSpaceState3DSW::intersect_shape(const RID &p_shape, const Trans return 0; } - Shape3DSW *shape = static_cast<PhysicsServer3DSW *>(PhysicsServer3D::get_singleton())->shape_owner.getornull(p_shape); + Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, 0); AABB aabb = p_xform.xform(shape->get_aabb()); @@ -232,7 +232,7 @@ int PhysicsDirectSpaceState3DSW::intersect_shape(const RID &p_shape, const Trans } bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transform &p_xform, const Vector3 &p_motion, real_t p_margin, real_t &p_closest_safe, real_t &p_closest_unsafe, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas, ShapeRestInfo *r_info) { - Shape3DSW *shape = static_cast<PhysicsServer3DSW *>(PhysicsServer3D::get_singleton())->shape_owner.getornull(p_shape); + Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, false); AABB aabb = p_xform.xform(shape->get_aabb()); @@ -340,7 +340,7 @@ bool PhysicsDirectSpaceState3DSW::collide_shape(RID p_shape, const Transform &p_ return false; } - Shape3DSW *shape = static_cast<PhysicsServer3DSW *>(PhysicsServer3D::get_singleton())->shape_owner.getornull(p_shape); + Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, 0); AABB aabb = p_shape_xform.xform(shape->get_aabb()); @@ -412,7 +412,7 @@ static void _rest_cbk_result(const Vector3 &p_point_A, const Vector3 &p_point_B, } bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform &p_shape_xform, real_t p_margin, ShapeRestInfo *r_info, const Set<RID> &p_exclude, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) { - Shape3DSW *shape = static_cast<PhysicsServer3DSW *>(PhysicsServer3D::get_singleton())->shape_owner.getornull(p_shape); + Shape3DSW *shape = PhysicsServer3DSW::singletonsw->shape_owner.getornull(p_shape); ERR_FAIL_COND_V(!shape, 0); AABB aabb = p_shape_xform.xform(shape->get_aabb()); @@ -468,9 +468,9 @@ bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform &p_shap } Vector3 PhysicsDirectSpaceState3DSW::get_closest_point_to_object_volume(RID p_object, const Vector3 p_point) const { - CollisionObject3DSW *obj = PhysicsServer3DSW::singleton->area_owner.getornull(p_object); + CollisionObject3DSW *obj = PhysicsServer3DSW::singletonsw->area_owner.getornull(p_object); if (!obj) { - obj = PhysicsServer3DSW::singleton->body_owner.getornull(p_object); + obj = PhysicsServer3DSW::singletonsw->body_owner.getornull(p_object); } ERR_FAIL_COND_V(!obj, Vector3()); |