summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--servers/physics_2d/body_pair_2d_sw.cpp43
-rw-r--r--servers/physics_2d/physics_server_2d_sw.cpp32
-rw-r--r--servers/physics_2d/space_2d_sw.cpp31
-rw-r--r--servers/physics_3d/space_3d_sw.cpp4
4 files changed, 47 insertions, 63 deletions
diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp
index 6a13453f9f..feced36a2b 100644
--- a/servers/physics_2d/body_pair_2d_sw.cpp
+++ b/servers/physics_2d/body_pair_2d_sw.cpp
@@ -288,21 +288,17 @@ bool BodyPair2DSW::setup(real_t p_step) {
if (A->is_shape_set_as_one_way_collision(shape_A)) {
Vector2 direction = xform_A.get_axis(1).normalized();
bool valid = false;
- if (B->get_linear_velocity().dot(direction) >= 0) {
- for (int i = 0; i < contact_count; i++) {
- Contact &c = contacts[i];
- if (!c.reused) {
- continue;
- }
- if (c.normal.dot(direction) > 0) { //greater (normal inverted)
- continue;
- }
-
- valid = true;
- break;
+ for (int i = 0; i < contact_count; i++) {
+ Contact &c = contacts[i];
+ if (!c.reused) {
+ continue;
}
+ if (c.normal.dot(direction) > -CMP_EPSILON) { //greater (normal inverted)
+ continue;
+ }
+ valid = true;
+ break;
}
-
if (!valid) {
collided = false;
oneway_disabled = true;
@@ -313,19 +309,16 @@ bool BodyPair2DSW::setup(real_t p_step) {
if (B->is_shape_set_as_one_way_collision(shape_B)) {
Vector2 direction = xform_B.get_axis(1).normalized();
bool valid = false;
- if (A->get_linear_velocity().dot(direction) >= 0) {
- for (int i = 0; i < contact_count; i++) {
- Contact &c = contacts[i];
- if (!c.reused) {
- continue;
- }
- if (c.normal.dot(direction) < 0) { //less (normal ok)
- continue;
- }
-
- valid = true;
- break;
+ for (int i = 0; i < contact_count; i++) {
+ Contact &c = contacts[i];
+ if (!c.reused) {
+ continue;
+ }
+ if (c.normal.dot(direction) < CMP_EPSILON) { //less (normal ok)
+ continue;
}
+ valid = true;
+ break;
}
if (!valid) {
collided = false;
diff --git a/servers/physics_2d/physics_server_2d_sw.cpp b/servers/physics_2d/physics_server_2d_sw.cpp
index 85e24ca537..c4e2489bef 100644
--- a/servers/physics_2d/physics_server_2d_sw.cpp
+++ b/servers/physics_2d/physics_server_2d_sw.cpp
@@ -149,24 +149,19 @@ void PhysicsServer2DSW::_shape_col_cbk(const Vector2 &p_point_A, const Vector2 &
return;
}
+ Vector2 rel_dir = (p_point_A - p_point_B);
+ real_t rel_length2 = rel_dir.length_squared();
if (cbk->valid_dir != Vector2()) {
- if (p_point_A.distance_squared_to(p_point_B) > cbk->valid_depth * cbk->valid_depth) {
- cbk->invalid_by_dir++;
- return;
- }
- Vector2 rel_dir = (p_point_A - p_point_B).normalized();
-
- if (cbk->valid_dir.dot(rel_dir) < Math_SQRT12) { //sqrt(2)/2.0 - 45 degrees
- cbk->invalid_by_dir++;
-
- /*
- print_line("A: "+p_point_A);
- print_line("B: "+p_point_B);
- print_line("discard too angled "+rtos(cbk->valid_dir.dot((p_point_A-p_point_B))));
- print_line("resnorm: "+(p_point_A-p_point_B).normalized());
- print_line("distance: "+rtos(p_point_A.distance_to(p_point_B)));
- */
- return;
+ if (cbk->valid_depth < 10e20) {
+ if (rel_length2 > cbk->valid_depth * cbk->valid_depth ||
+ (rel_length2 > CMP_EPSILON && cbk->valid_dir.dot(rel_dir.normalized()) < CMP_EPSILON)) {
+ cbk->invalid_by_dir++;
+ return;
+ }
+ } else {
+ if (rel_length2 > 0 && cbk->valid_dir.dot(rel_dir.normalized()) < CMP_EPSILON) {
+ return;
+ }
}
}
@@ -182,8 +177,7 @@ void PhysicsServer2DSW::_shape_col_cbk(const Vector2 &p_point_A, const Vector2 &
}
}
- real_t d = p_point_A.distance_squared_to(p_point_B);
- if (d < min_depth) {
+ if (rel_length2 < min_depth) {
return;
}
cbk->ptr[min_depth_idx * 2 + 0] = p_point_A;
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index c2a6dc828e..ad5dca37e4 100644
--- a/servers/physics_2d/space_2d_sw.cpp
+++ b/servers/physics_2d/space_2d_sw.cpp
@@ -376,25 +376,25 @@ struct _RestCallbackData2D {
Vector2 best_normal;
real_t best_len;
Vector2 valid_dir;
- real_t valid_depth;
real_t min_allowed_depth;
};
static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_userdata) {
_RestCallbackData2D *rd = (_RestCallbackData2D *)p_userdata;
- if (rd->valid_dir != Vector2()) {
- if (p_point_A.distance_squared_to(p_point_B) > rd->valid_depth * rd->valid_depth) {
- return;
- }
- if (rd->valid_dir.dot((p_point_A - p_point_B).normalized()) < Math_PI * 0.25) {
- return;
- }
- }
-
Vector2 contact_rel = p_point_B - p_point_A;
real_t len = contact_rel.length();
+ if (len == 0) {
+ return;
+ }
+
+ Vector2 normal = contact_rel / len;
+
+ if (rd->valid_dir != Vector2() && rd->valid_dir.dot(normal) > -CMP_EPSILON) {
+ return;
+ }
+
if (len < rd->min_allowed_depth) {
return;
}
@@ -405,7 +405,7 @@ static void _rest_cbk_result(const Vector2 &p_point_A, const Vector2 &p_point_B,
rd->best_len = len;
rd->best_contact = p_point_B;
- rd->best_normal = contact_rel / len;
+ rd->best_normal = normal;
rd->best_object = rd->object;
rd->best_shape = rd->shape;
rd->best_local_shape = rd->local_shape;
@@ -440,7 +440,6 @@ bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_sh
}
rcd.valid_dir = Vector2();
- rcd.valid_depth = 0;
rcd.object = col_obj;
rcd.shape = shape_idx;
rcd.local_shape = 0;
@@ -643,7 +642,7 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t
Vector2 a = sr[k * 2 + 0];
Vector2 b = sr[k * 2 + 1];
- recover_motion += (b - a) * 0.4;
+ recover_motion += (b - a) / cbk.amount;
float depth = a.distance_to(b);
if (depth > result.collision_depth) {
@@ -850,7 +849,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
for (int i = 0; i < cbk.amount; i++) {
Vector2 a = sr[i * 2 + 0];
Vector2 b = sr[i * 2 + 1];
- recover_motion += (b - a) * 0.4;
+ recover_motion += (b - a) / cbk.amount;
}
if (recover_motion == Vector2()) {
@@ -1002,7 +1001,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
best_shape = -1; //no best shape with cast, reset to -1
}
- {
+ if (safe < 1) {
//it collided, let's get the rest info in unsafe advance
Transform2D ugt = body_transform;
ugt.elements[2] += p_motion * unsafe;
@@ -1061,10 +1060,8 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
if (col_obj->is_shape_set_as_one_way_collision(shape_idx)) {
rcd.valid_dir = col_obj_shape_xform.get_axis(1).normalized();
- rcd.valid_depth = 10e20;
} else {
rcd.valid_dir = Vector2();
- rcd.valid_depth = 0;
}
rcd.object = col_obj;
diff --git a/servers/physics_3d/space_3d_sw.cpp b/servers/physics_3d/space_3d_sw.cpp
index 2b2b5122da..0395a3339c 100644
--- a/servers/physics_3d/space_3d_sw.cpp
+++ b/servers/physics_3d/space_3d_sw.cpp
@@ -649,7 +649,7 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform &p_tra
Vector3 a = sr[k * 2 + 0];
Vector3 b = sr[k * 2 + 1];
- recover_motion += (b - a) * 0.4;
+ recover_motion += (b - a) / cbk.amount;
float depth = a.distance_to(b);
if (depth > result.collision_depth) {
@@ -791,7 +791,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform &p_from, cons
for (int i = 0; i < cbk.amount; i++) {
Vector3 a = sr[i * 2 + 0];
Vector3 b = sr[i * 2 + 1];
- recover_motion += (b - a) * 0.4;
+ recover_motion += (b - a) / cbk.amount;
}
if (recover_motion == Vector3()) {
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-04 17:59:04 +0000