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.cpp111
1 files changed, 66 insertions, 45 deletions
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index 1380e57b57..82da5774db 100644
--- a/servers/physics_2d/space_2d_sw.cpp
+++ b/servers/physics_2d/space_2d_sw.cpp
@@ -84,10 +84,6 @@ int PhysicsDirectSpaceState2DSW::_intersect_point_impl(const Vector2 &p_point, S
int shape_idx = space->intersection_query_subindex_results[i];
- if (col_obj->is_shape_set_as_disabled(shape_idx)) {
- continue;
- }
-
Shape2DSW *shape = col_obj->get_shape(shape_idx);
Vector2 local_point = (col_obj->get_transform() * col_obj->get_shape_transform(shape_idx)).affine_inverse().xform(p_point);
@@ -233,10 +229,6 @@ int PhysicsDirectSpaceState2DSW::intersect_shape(const RID &p_shape, const Trans
const CollisionObject2DSW *col_obj = space->intersection_query_results[i];
int shape_idx = space->intersection_query_subindex_results[i];
- if (col_obj->is_shape_set_as_disabled(shape_idx)) {
- continue;
- }
-
if (!CollisionSolver2DSW::solve(shape, p_xform, p_motion, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), nullptr, nullptr, nullptr, p_margin)) {
continue;
}
@@ -280,10 +272,6 @@ bool PhysicsDirectSpaceState2DSW::cast_motion(const RID &p_shape, const Transfor
const CollisionObject2DSW *col_obj = space->intersection_query_results[i];
int shape_idx = space->intersection_query_subindex_results[i];
- if (col_obj->is_shape_set_as_disabled(shape_idx)) {
- continue;
- }
-
Transform2D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
//test initial overlap, does it collide if going all the way?
if (!CollisionSolver2DSW::solve(shape, p_xform, p_motion, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, nullptr, p_margin)) {
@@ -295,22 +283,38 @@ bool PhysicsDirectSpaceState2DSW::cast_motion(const RID &p_shape, const Transfor
continue;
}
- //just do kinematic solving
- real_t low = 0;
- real_t hi = 1;
Vector2 mnormal = p_motion.normalized();
+ //just do kinematic solving
+ real_t low = 0.0;
+ real_t hi = 1.0;
+ real_t fraction_coeff = 0.5;
for (int j = 0; j < 8; j++) { //steps should be customizable..
-
- real_t ofs = (low + hi) * 0.5;
+ real_t fraction = low + (hi - low) * fraction_coeff;
Vector2 sep = mnormal; //important optimization for this to work fast enough
- bool collided = CollisionSolver2DSW::solve(shape, p_xform, p_motion * ofs, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, &sep, p_margin);
+ bool collided = CollisionSolver2DSW::solve(shape, p_xform, p_motion * fraction, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), nullptr, nullptr, &sep, p_margin);
if (collided) {
- hi = ofs;
+ hi = fraction;
+ if ((j == 0) || (low > 0.0)) { // Did it not collide before?
+ // When alternating or first iteration, use dichotomy.
+ fraction_coeff = 0.5;
+ } else {
+ // When colliding again, converge faster towards low fraction
+ // for more accurate results with long motions that collide near the start.
+ fraction_coeff = 0.25;
+ }
} else {
- low = ofs;
+ low = fraction;
+ if ((j == 0) || (hi < 1.0)) { // Did it collide before?
+ // When alternating or first iteration, use dichotomy.
+ fraction_coeff = 0.5;
+ } else {
+ // When not colliding again, converge faster towards high fraction
+ // for more accurate results with long motions that collide near the end.
+ fraction_coeff = 0.75;
+ }
}
}
@@ -365,10 +369,6 @@ bool PhysicsDirectSpaceState2DSW::collide_shape(RID p_shape, const Transform2D &
int shape_idx = space->intersection_query_subindex_results[i];
- if (col_obj->is_shape_set_as_disabled(shape_idx)) {
- continue;
- }
-
cbk.valid_dir = Vector2();
cbk.valid_depth = 0;
@@ -460,10 +460,6 @@ bool PhysicsDirectSpaceState2DSW::rest_info(RID p_shape, const Transform2D &p_sh
int shape_idx = space->intersection_query_subindex_results[i];
- if (col_obj->is_shape_set_as_disabled(shape_idx)) {
- continue;
- }
-
rcd.valid_dir = Vector2();
rcd.object = col_obj;
rcd.shape = shape_idx;
@@ -516,8 +512,6 @@ int Space2DSW::_cull_aabb_for_body(Body2DSW *p_body, const Rect2 &p_aabb) {
keep = false;
} else if (static_cast<Body2DSW *>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) {
keep = false;
- } else if (static_cast<Body2DSW *>(intersection_query_results[i])->is_shape_set_as_disabled(intersection_query_subindex_results[i])) {
- keep = false;
}
if (!keep) {
@@ -540,7 +534,7 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t
bool shapes_found = false;
for (int i = 0; i < p_body->get_shape_count(); i++) {
- if (p_body->is_shape_set_as_disabled(i)) {
+ if (p_body->is_shape_disabled(i)) {
continue;
}
@@ -592,7 +586,7 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t
int amount = _cull_aabb_for_body(p_body, body_aabb);
for (int j = 0; j < p_body->get_shape_count(); j++) {
- if (p_body->is_shape_set_as_disabled(j)) {
+ if (p_body->is_shape_disabled(j)) {
continue;
}
@@ -715,7 +709,7 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t
return rays_found;
}
-bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer2D::MotionResult *r_result, bool p_exclude_raycast_shapes) {
+bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, const Vector2 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer2D::MotionResult *r_result, bool p_exclude_raycast_shapes, const Set<RID> &p_exclude) {
//give me back regular physics engine logic
//this is madness
//and most people using this function will think
@@ -732,7 +726,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
bool shapes_found = false;
for (int i = 0; i < p_body->get_shape_count(); i++) {
- if (p_body->is_shape_set_as_disabled(i)) {
+ if (p_body->is_shape_disabled(i)) {
continue;
}
@@ -795,7 +789,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
int amount = _cull_aabb_for_body(p_body, body_aabb);
for (int j = 0; j < p_body->get_shape_count(); j++) {
- if (p_body->is_shape_set_as_disabled(j)) {
+ if (p_body->is_shape_disabled(j)) {
continue;
}
@@ -807,6 +801,9 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j);
for (int i = 0; i < amount; i++) {
const CollisionObject2DSW *col_obj = intersection_query_results[i];
+ if (p_exclude.has(col_obj->get_self())) {
+ continue;
+ }
int shape_idx = intersection_query_subindex_results[i];
if (CollisionObject2DSW::TYPE_BODY == col_obj->get_type()) {
@@ -918,7 +915,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
int amount = _cull_aabb_for_body(p_body, motion_aabb);
for (int body_shape_idx = 0; body_shape_idx < p_body->get_shape_count(); body_shape_idx++) {
- if (p_body->is_shape_set_as_disabled(body_shape_idx)) {
+ if (p_body->is_shape_disabled(body_shape_idx)) {
continue;
}
@@ -936,6 +933,9 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
for (int i = 0; i < amount; i++) {
const CollisionObject2DSW *col_obj = intersection_query_results[i];
+ if (p_exclude.has(col_obj->get_self())) {
+ continue;
+ }
int col_shape_idx = intersection_query_subindex_results[i];
Shape2DSW *against_shape = col_obj->get_shape(col_shape_idx);
@@ -979,20 +979,35 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
}
//just do kinematic solving
- real_t low = 0;
- real_t hi = 1;
-
+ real_t low = 0.0;
+ real_t hi = 1.0;
+ real_t fraction_coeff = 0.5;
for (int k = 0; k < 8; k++) { //steps should be customizable..
-
- real_t ofs = (low + hi) * 0.5;
+ real_t fraction = low + (hi - low) * fraction_coeff;
Vector2 sep = motion_normal; //important optimization for this to work fast enough
- bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * ofs, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, &sep, 0);
+ bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * fraction, against_shape, col_obj_shape_xform, Vector2(), nullptr, nullptr, &sep, 0);
if (collided) {
- hi = ofs;
+ hi = fraction;
+ if ((k == 0) || (low > 0.0)) { // Did it not collide before?
+ // When alternating or first iteration, use dichotomy.
+ fraction_coeff = 0.5;
+ } else {
+ // When colliding again, converge faster towards low fraction
+ // for more accurate results with long motions that collide near the start.
+ fraction_coeff = 0.25;
+ }
} else {
- low = ofs;
+ low = fraction;
+ if ((k == 0) || (hi < 1.0)) { // Did it collide before?
+ // When alternating or first iteration, use dichotomy.
+ fraction_coeff = 0.5;
+ } else {
+ // When not colliding again, converge faster towards high fraction
+ // for more accurate results with long motions that collide near the end.
+ fraction_coeff = 0.75;
+ }
}
}
@@ -1060,7 +1075,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
int to_shape = best_shape != -1 ? best_shape + 1 : p_body->get_shape_count();
for (int j = from_shape; j < to_shape; j++) {
- if (p_body->is_shape_set_as_disabled(j)) {
+ if (p_body->is_shape_disabled(j)) {
continue;
}
@@ -1077,6 +1092,9 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
for (int i = 0; i < amount; i++) {
const CollisionObject2DSW *col_obj = intersection_query_results[i];
+ if (p_exclude.has(col_obj->get_self())) {
+ continue;
+ }
int shape_idx = intersection_query_subindex_results[i];
if (CollisionObject2DSW::TYPE_BODY == col_obj->get_type()) {
@@ -1142,6 +1160,9 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
r_result->collision_local_shape = rcd.best_local_shape;
r_result->collision_normal = rcd.best_normal;
r_result->collision_point = rcd.best_contact;
+ 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);
generated by cgit v1.2.3 (git 2.25.1) at 2025-06-03 14:58:45 +0000