summaryrefslogtreecommitdiff
path: root/servers/physics_2d/space_2d_sw.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics_2d/space_2d_sw.cpp')
-rw-r--r--servers/physics_2d/space_2d_sw.cpp37
1 files changed, 17 insertions, 20 deletions
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index b9b26eb21d..5cd9bf3223 100644
--- a/servers/physics_2d/space_2d_sw.cpp
+++ b/servers/physics_2d/space_2d_sw.cpp
@@ -34,6 +34,9 @@
#include "core/os/os.h"
#include "core/templates/pair.h"
#include "physics_server_2d_sw.h"
+
+#define TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR 0.05
+
_FORCE_INLINE_ static bool _can_collide_with(CollisionObject2DSW *p_object, uint32_t p_collision_mask, bool p_collide_with_bodies, bool p_collide_with_areas) {
if (!(p_object->get_collision_layer() & p_collision_mask)) {
return false;
@@ -102,7 +105,6 @@ int PhysicsDirectSpaceState2DSW::_intersect_point_impl(const Vector2 &p_point, S
}
r_results[cc].rid = col_obj->get_self();
r_results[cc].shape = shape_idx;
- r_results[cc].metadata = col_obj->get_shape_metadata(shape_idx);
cc++;
}
@@ -190,7 +192,6 @@ bool PhysicsDirectSpaceState2DSW::intersect_ray(const Vector2 &p_from, const Vec
r_result.collider = ObjectDB::get_instance(r_result.collider_id);
}
r_result.normal = res_normal;
- r_result.metadata = res_obj->get_shape_metadata(res_shape);
r_result.position = res_point;
r_result.rid = res_obj->get_self();
r_result.shape = res_shape;
@@ -203,7 +204,7 @@ int PhysicsDirectSpaceState2DSW::intersect_shape(const RID &p_shape, const Trans
return 0;
}
- Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.getornull(p_shape);
+ Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.get_or_null(p_shape);
ERR_FAIL_COND_V(!shape, 0);
Rect2 aabb = p_xform.xform(shape->get_aabb());
@@ -239,7 +240,6 @@ int PhysicsDirectSpaceState2DSW::intersect_shape(const RID &p_shape, const Trans
}
r_results[cc].rid = col_obj->get_self();
r_results[cc].shape = shape_idx;
- r_results[cc].metadata = col_obj->get_shape_metadata(shape_idx);
cc++;
}
@@ -248,7 +248,7 @@ int PhysicsDirectSpaceState2DSW::intersect_shape(const RID &p_shape, const Trans
}
bool PhysicsDirectSpaceState2DSW::cast_motion(const RID &p_shape, const Transform2D &p_xform, const Vector2 &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) {
- Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.getornull(p_shape);
+ Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.get_or_null(p_shape);
ERR_FAIL_COND_V(!shape, false);
Rect2 aabb = p_xform.xform(shape->get_aabb());
@@ -335,7 +335,7 @@ bool PhysicsDirectSpaceState2DSW::collide_shape(RID p_shape, const Transform2D &
return false;
}
- Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.getornull(p_shape);
+ Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.get_or_null(p_shape);
ERR_FAIL_COND_V(!shape, 0);
Rect2 aabb = p_shape_xform.xform(shape->get_aabb());
@@ -432,9 +432,11 @@ static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B,
}
bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_shape_xform, const Vector2 &p_motion, 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) {
- Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.getornull(p_shape);
+ Shape2DSW *shape = PhysicsServer2DSW::singletonsw->shape_owner.get_or_null(p_shape);
ERR_FAIL_COND_V(!shape, 0);
+ real_t min_contact_depth = p_margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
+
Rect2 aabb = p_shape_xform.xform(shape->get_aabb());
aabb = aabb.merge(Rect2(aabb.position + p_motion, aabb.size)); //motion
aabb = aabb.grow(p_margin);
@@ -445,7 +447,7 @@ bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_sh
rcd.best_len = 0;
rcd.best_object = nullptr;
rcd.best_shape = 0;
- rcd.min_allowed_depth = space->test_motion_min_contact_depth;
+ rcd.min_allowed_depth = min_contact_depth;
for (int i = 0; i < amount; i++) {
if (!_can_collide_with(space->intersection_query_results[i], p_collision_mask, p_collide_with_bodies, p_collide_with_areas)) {
@@ -479,7 +481,6 @@ bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_sh
r_info->normal = rcd.best_normal;
r_info->point = rcd.best_contact;
r_info->rid = rcd.best_object->get_self();
- r_info->metadata = rcd.best_object->get_shape_metadata(rcd.best_shape);
if (rcd.best_object->get_type() == CollisionObject2DSW::TYPE_BODY) {
const Body2DSW *body = static_cast<const Body2DSW *>(rcd.best_object);
Vector2 rel_vec = r_info->point - (body->get_transform().get_origin() + body->get_center_of_mass());
@@ -569,6 +570,8 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
ExcludedShapeSW excluded_shape_pairs[max_excluded_shape_pairs];
int excluded_shape_pair_count = 0;
+ real_t min_contact_depth = p_margin * TEST_MOTION_MIN_CONTACT_DEPTH_FACTOR;
+
real_t motion_length = p_motion.length();
Vector2 motion_normal = p_motion / motion_length;
@@ -671,6 +674,8 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
break;
}
+ recovered = true;
+
Vector2 recover_motion;
for (int i = 0; i < cbk.amount; i++) {
Vector2 a = sr[i * 2 + 0];
@@ -682,9 +687,9 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
// Compute depth on recovered motion.
real_t depth = n.dot(a + recover_motion) - d;
- if (depth > 0.0) {
+ if (depth > min_contact_depth + CMP_EPSILON) {
// Only recover if there is penetration.
- recover_motion -= n * depth * 0.4;
+ recover_motion -= n * (depth - min_contact_depth) * 0.4;
}
}
@@ -693,8 +698,6 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
break;
}
- recovered = true;
-
body_transform.elements[2] += recover_motion;
body_aabb.position += recover_motion;
@@ -870,7 +873,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
rcd.best_shape = 0;
// Allowed depth can't be lower than motion length, in order to handle contacts at low speed.
- rcd.min_allowed_depth = MIN(motion_length, test_motion_min_contact_depth);
+ rcd.min_allowed_depth = MIN(motion_length, min_contact_depth);
int from_shape = best_shape != -1 ? best_shape : 0;
int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count();
@@ -954,7 +957,6 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
r_result->collision_depth = rcd.best_len;
r_result->collision_safe_fraction = safe;
r_result->collision_unsafe_fraction = unsafe;
- r_result->collider_metadata = rcd.best_object->get_shape_metadata(rcd.best_shape);
const Body2DSW *body = static_cast<const Body2DSW *>(rcd.best_object);
Vector2 rel_vec = r_result->collision_point - (body->get_transform().get_origin() + body->get_center_of_mass());
@@ -1141,9 +1143,6 @@ void Space2DSW::set_param(PhysicsServer2D::SpaceParameter p_param, real_t p_valu
case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS:
constraint_bias = p_value;
break;
- case PhysicsServer2D::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH:
- test_motion_min_contact_depth = p_value;
- break;
}
}
@@ -1163,8 +1162,6 @@ real_t Space2DSW::get_param(PhysicsServer2D::SpaceParameter p_param) const {
return body_time_to_sleep;
case PhysicsServer2D::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS:
return constraint_bias;
- case PhysicsServer2D::SPACE_PARAM_TEST_MOTION_MIN_CONTACT_DEPTH:
- return test_motion_min_contact_depth;
}
return 0;
}
generated by cgit v1.2.3 (git 2.25.1) at 2025-03-31 06:43:14 +0000