summaryrefslogtreecommitdiff
path: root/servers/physics_3d/space_3d_sw.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics_3d/space_3d_sw.cpp')
-rw-r--r--servers/physics_3d/space_3d_sw.cpp198
1 files changed, 13 insertions, 185 deletions
diff --git a/servers/physics_3d/space_3d_sw.cpp b/servers/physics_3d/space_3d_sw.cpp
index bc149648d7..730460d66a 100644
--- a/servers/physics_3d/space_3d_sw.cpp
+++ b/servers/physics_3d/space_3d_sw.cpp
@@ -549,7 +549,7 @@ int Space3DSW::_cull_aabb_for_body(Body3DSW *p_body, const AABB &p_aabb) {
keep = false;
} else if (intersection_query_results[i]->get_type() == CollisionObject3DSW::TYPE_SOFT_BODY) {
keep = false;
- } else if (!p_body->layer_in_mask(static_cast<Body3DSW *>(intersection_query_results[i]))) {
+ } else if (!p_body->collides_with(static_cast<Body3DSW *>(intersection_query_results[i]))) {
keep = false;
} else if (static_cast<Body3DSW *>(intersection_query_results[i])->has_exception(p_body->get_self()) || p_body->has_exception(intersection_query_results[i]->get_self())) {
keep = false;
@@ -569,158 +569,7 @@ int Space3DSW::_cull_aabb_for_body(Body3DSW *p_body, const AABB &p_aabb) {
return amount;
}
-int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform3D &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, PhysicsServer3D::SeparationResult *r_results, int p_result_max, real_t p_margin) {
- AABB body_aabb;
-
- bool shapes_found = false;
-
- for (int i = 0; i < p_body->get_shape_count(); i++) {
- if (p_body->is_shape_disabled(i)) {
- continue;
- }
-
- if (!shapes_found) {
- body_aabb = p_body->get_shape_aabb(i);
- shapes_found = true;
- } else {
- body_aabb = body_aabb.merge(p_body->get_shape_aabb(i));
- }
- }
-
- if (!shapes_found) {
- return 0;
- }
- // Undo the currently transform the physics server is aware of and apply the provided one
- body_aabb = p_transform.xform(p_body->get_inv_transform().xform(body_aabb));
- body_aabb = body_aabb.grow(p_margin);
-
- Transform3D body_transform = p_transform;
-
- for (int i = 0; i < p_result_max; i++) {
- //reset results
- r_results[i].collision_depth = 0;
- }
-
- int rays_found = 0;
-
- {
- // raycast AND separate
-
- const int max_results = 32;
- int recover_attempts = 4;
- Vector3 sr[max_results * 2];
- PhysicsServer3DSW::CollCbkData cbk;
- cbk.max = max_results;
- PhysicsServer3DSW::CollCbkData *cbkptr = &cbk;
- CollisionSolver3DSW::CallbackResult cbkres = PhysicsServer3DSW::_shape_col_cbk;
-
- do {
- Vector3 recover_motion;
-
- bool collided = false;
-
- 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_disabled(j)) {
- continue;
- }
-
- Shape3DSW *body_shape = p_body->get_shape(j);
-
- if (body_shape->get_type() != PhysicsServer3D::SHAPE_RAY) {
- continue;
- }
-
- Transform3D body_shape_xform = body_transform * p_body->get_shape_transform(j);
-
- for (int i = 0; i < amount; i++) {
- const CollisionObject3DSW *col_obj = intersection_query_results[i];
- int shape_idx = intersection_query_subindex_results[i];
-
- cbk.amount = 0;
- cbk.ptr = sr;
-
- if (CollisionObject3DSW::TYPE_BODY == col_obj->get_type()) {
- const Body3DSW *b = static_cast<const Body3DSW *>(col_obj);
- if (p_infinite_inertia && PhysicsServer3D::BODY_MODE_STATIC != b->get_mode() && PhysicsServer3D::BODY_MODE_KINEMATIC != b->get_mode()) {
- continue;
- }
- }
-
- Shape3DSW *against_shape = col_obj->get_shape(shape_idx);
- if (CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, against_shape, col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, p_margin)) {
- if (cbk.amount > 0) {
- collided = true;
- }
-
- int ray_index = -1; //reuse shape
- for (int k = 0; k < rays_found; k++) {
- if (r_results[k].collision_local_shape == j) {
- ray_index = k;
- }
- }
-
- if (ray_index == -1 && rays_found < p_result_max) {
- ray_index = rays_found;
- rays_found++;
- }
-
- if (ray_index != -1) {
- PhysicsServer3D::SeparationResult &result = r_results[ray_index];
-
- for (int k = 0; k < cbk.amount; k++) {
- Vector3 a = sr[k * 2 + 0];
- Vector3 b = sr[k * 2 + 1];
-
- recover_motion += (b - a) / cbk.amount;
-
- real_t depth = a.distance_to(b);
- if (depth > result.collision_depth) {
- result.collision_depth = depth;
- result.collision_point = b;
- result.collision_normal = (b - a).normalized();
- result.collision_local_shape = j;
- result.collider = col_obj->get_self();
- result.collider_id = col_obj->get_instance_id();
- result.collider_shape = shape_idx;
- //result.collider_metadata = col_obj->get_shape_metadata(shape_idx);
- if (col_obj->get_type() == CollisionObject3DSW::TYPE_BODY) {
- Body3DSW *body = (Body3DSW *)col_obj;
- Vector3 rel_vec = b - (body->get_transform().origin + body->get_center_of_mass());
- result.collider_velocity = body->get_linear_velocity() + (body->get_angular_velocity()).cross(rel_vec);
- }
- }
- }
- }
- }
- }
- }
-
- if (!collided || recover_motion == Vector3()) {
- break;
- }
-
- body_transform.origin += recover_motion;
- body_aabb.position += recover_motion;
-
- recover_attempts--;
- } while (recover_attempts);
- }
-
- //optimize results (remove non colliding)
- for (int i = 0; i < rays_found; i++) {
- if (r_results[i].collision_depth == 0) {
- rays_found--;
- SWAP(r_results[i], r_results[rays_found]);
- }
- }
-
- r_recover_motion = body_transform.origin - p_transform.origin;
- return rays_found;
-}
-
-bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, const Vector3 &p_motion, bool p_infinite_inertia, real_t p_margin, PhysicsServer3D::MotionResult *r_result, bool p_exclude_raycast_shapes) {
+bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, const Vector3 &p_motion, real_t p_margin, PhysicsServer3D::MotionResult *r_result, const Set<RID> &p_exclude) {
//give me back regular physics engine logic
//this is madness
//and most people using this function will think
@@ -795,21 +644,15 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
Transform3D body_shape_xform = body_transform * p_body->get_shape_transform(j);
Shape3DSW *body_shape = p_body->get_shape(j);
- if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer3D::SHAPE_RAY) {
- continue;
- }
for (int i = 0; i < amount; i++) {
const CollisionObject3DSW *col_obj = intersection_query_results[i];
- int shape_idx = intersection_query_subindex_results[i];
-
- if (CollisionObject3DSW::TYPE_BODY == col_obj->get_type()) {
- const Body3DSW *b = static_cast<const Body3DSW *>(col_obj);
- if (p_infinite_inertia && PhysicsServer3D::BODY_MODE_STATIC != b->get_mode() && PhysicsServer3D::BODY_MODE_KINEMATIC != b->get_mode()) {
- continue;
- }
+ if (p_exclude.has(col_obj->get_self())) {
+ continue;
}
+ int shape_idx = intersection_query_subindex_results[i];
+
if (CollisionSolver3DSW::solve_static(body_shape, body_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), cbkres, cbkptr, nullptr, p_margin)) {
collided = cbk.amount > 0;
}
@@ -874,10 +717,6 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
Transform3D body_shape_xform = body_transform * p_body->get_shape_transform(j);
Shape3DSW *body_shape = p_body->get_shape(j);
- if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer3D::SHAPE_RAY) {
- continue;
- }
-
Transform3D body_shape_xform_inv = body_shape_xform.affine_inverse();
MotionShape3DSW mshape;
mshape.shape = body_shape;
@@ -890,15 +729,12 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
for (int i = 0; i < amount; i++) {
const CollisionObject3DSW *col_obj = intersection_query_results[i];
- int shape_idx = intersection_query_subindex_results[i];
-
- if (CollisionObject3DSW::TYPE_BODY == col_obj->get_type()) {
- const Body3DSW *b = static_cast<const Body3DSW *>(col_obj);
- if (p_infinite_inertia && PhysicsServer3D::BODY_MODE_STATIC != b->get_mode() && PhysicsServer3D::BODY_MODE_KINEMATIC != b->get_mode()) {
- continue;
- }
+ if (p_exclude.has(col_obj->get_self())) {
+ continue;
}
+ int shape_idx = intersection_query_subindex_results[i];
+
//test initial overlap, does it collide if going all the way?
Vector3 point_A, point_B;
Vector3 sep_axis = motion_normal;
@@ -1005,24 +841,16 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
Transform3D body_shape_xform = ugt * p_body->get_shape_transform(j);
Shape3DSW *body_shape = p_body->get_shape(j);
- if (p_exclude_raycast_shapes && body_shape->get_type() == PhysicsServer3D::SHAPE_RAY) {
- continue;
- }
-
body_aabb.position += p_motion * unsafe;
int amount = _cull_aabb_for_body(p_body, body_aabb);
for (int i = 0; i < amount; i++) {
const CollisionObject3DSW *col_obj = intersection_query_results[i];
- int shape_idx = intersection_query_subindex_results[i];
-
- if (CollisionObject3DSW::TYPE_BODY == col_obj->get_type()) {
- const Body3DSW *b = static_cast<const Body3DSW *>(col_obj);
- if (p_infinite_inertia && PhysicsServer3D::BODY_MODE_STATIC != b->get_mode() && PhysicsServer3D::BODY_MODE_KINEMATIC != b->get_mode()) {
- continue;
- }
+ if (p_exclude.has(col_obj->get_self())) {
+ continue;
}
+ int shape_idx = intersection_query_subindex_results[i];
rcd.object = col_obj;
rcd.shape = shape_idx;
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-30 19:53:56 +0000