Merge pull request #49844 from nekomatata/physics-disabled-shapes

Fix and clean disabled shapes handling in godot physics servers

12 files changed, 76 insertions, 125 deletions

diff --git a/servers/physics_2d/area_pair_2d_sw.cpp b/servers/physics_2d/area_pair_2d_sw.cpp
index eb9fc0e800..5cc5bae09b 100644
--- a/servers/physics_2d/area_pair_2d_sw.cpp
+++ b/servers/physics_2d/area_pair_2d_sw.cpp
@@ -33,10 +33,7 @@
 
 bool AreaPair2DSW::setup(real_t p_step) {
 	bool result = false;
-
-	if (area->is_shape_set_as_disabled(area_shape) || body->is_shape_set_as_disabled(body_shape)) {
-		result = false;
-	} else if (area->test_collision_mask(body) && CollisionSolver2DSW::solve(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), Vector2(), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), Vector2(), nullptr, this)) {
+	if (area->test_collision_mask(body) && CollisionSolver2DSW::solve(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), Vector2(), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), Vector2(), nullptr, this)) {
 		result = true;
 	}
 
@@ -113,9 +110,7 @@ AreaPair2DSW::~AreaPair2DSW() {
 
 bool Area2Pair2DSW::setup(real_t p_step) {
 	bool result = false;
-	if (area_a->is_shape_set_as_disabled(shape_a) || area_b->is_shape_set_as_disabled(shape_b)) {
-		result = false;
-	} else if (area_a->test_collision_mask(area_b) && CollisionSolver2DSW::solve(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), Vector2(), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), Vector2(), nullptr, this)) {
+	if (area_a->test_collision_mask(area_b) && CollisionSolver2DSW::solve(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), Vector2(), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), Vector2(), nullptr, this)) {
 		result = true;
 	}
 
diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp
index ff31825b83..e5eb374fa3 100644
--- a/servers/physics_2d/body_pair_2d_sw.cpp
+++ b/servers/physics_2d/body_pair_2d_sw.cpp
@@ -244,11 +244,6 @@ bool BodyPair2DSW::setup(real_t p_step) {
 		}
 	}
 
-	if (A->is_shape_set_as_disabled(shape_A) || B->is_shape_set_as_disabled(shape_B)) {
-		collided = false;
-		return false;
-	}
-
 	//use local A coordinates to avoid numerical issues on collision detection
 	offset_B = B->get_transform().get_origin() - A->get_transform().get_origin();
 
diff --git a/servers/physics_2d/collision_object_2d_sw.cpp b/servers/physics_2d/collision_object_2d_sw.cpp
index fa87dc1f3f..5d1ef83165 100644
--- a/servers/physics_2d/collision_object_2d_sw.cpp
+++ b/servers/physics_2d/collision_object_2d_sw.cpp
@@ -47,8 +47,6 @@ void CollisionObject2DSW::add_shape(Shape2DSW *p_shape, const Transform2D &p_tra
 	if (!pending_shape_update_list.in_list()) {
 		PhysicsServer2DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list);
 	}
-	// _update_shapes();
-	// _shapes_changed();
 }
 
 void CollisionObject2DSW::set_shape(int p_index, Shape2DSW *p_shape) {
@@ -61,8 +59,6 @@ void CollisionObject2DSW::set_shape(int p_index, Shape2DSW *p_shape) {
 	if (!pending_shape_update_list.in_list()) {
 		PhysicsServer2DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list);
 	}
-	// _update_shapes();
-	// _shapes_changed();
 }
 
 void CollisionObject2DSW::set_shape_metadata(int p_index, const Variant &p_metadata) {
@@ -79,11 +75,9 @@ void CollisionObject2DSW::set_shape_transform(int p_index, const Transform2D &p_
 	if (!pending_shape_update_list.in_list()) {
 		PhysicsServer2DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list);
 	}
-	// _update_shapes();
-	// _shapes_changed();
 }
 
-void CollisionObject2DSW::set_shape_as_disabled(int p_idx, bool p_disabled) {
+void CollisionObject2DSW::set_shape_disabled(int p_idx, bool p_disabled) {
 	ERR_FAIL_INDEX(p_idx, shapes.size());
 
 	CollisionObject2DSW::Shape &shape = shapes.write[p_idx];
@@ -103,12 +97,10 @@ void CollisionObject2DSW::set_shape_as_disabled(int p_idx, bool p_disabled) {
 		if (!pending_shape_update_list.in_list()) {
 			PhysicsServer2DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list);
 		}
-		//_update_shapes();
 	} else if (!p_disabled && shape.bpid == 0) {
 		if (!pending_shape_update_list.in_list()) {
 			PhysicsServer2DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list);
 		}
-		//_update_shapes(); // automatically adds shape with bpid == 0
 	}
 }
 
@@ -177,7 +169,6 @@ void CollisionObject2DSW::_update_shapes() {
 
 	for (int i = 0; i < shapes.size(); i++) {
 		Shape &s = shapes.write[i];
-
 		if (s.disabled) {
 			continue;
 		}
diff --git a/servers/physics_2d/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h
index c395e59694..67da758d14 100644
--- a/servers/physics_2d/collision_object_2d_sw.h
+++ b/servers/physics_2d/collision_object_2d_sw.h
@@ -118,10 +118,6 @@ public:
 	void set_shape_metadata(int p_index, const Variant &p_metadata);
 
 	_FORCE_INLINE_ int get_shape_count() const { return shapes.size(); }
-	_FORCE_INLINE_ bool is_shape_disabled(int p_index) const {
-		CRASH_BAD_INDEX(p_index, shapes.size());
-		return shapes[p_index].disabled;
-	}
 	_FORCE_INLINE_ Shape2DSW *get_shape(int p_index) const {
 		CRASH_BAD_INDEX(p_index, shapes.size());
 		return shapes[p_index].shape;
@@ -147,9 +143,9 @@ public:
 	_FORCE_INLINE_ const Transform2D &get_inv_transform() const { return inv_transform; }
 	_FORCE_INLINE_ Space2DSW *get_space() const { return space; }
 
-	void set_shape_as_disabled(int p_idx, bool p_disabled);
-	_FORCE_INLINE_ bool is_shape_set_as_disabled(int p_idx) const {
-		CRASH_BAD_INDEX(p_idx, shapes.size());
+	void set_shape_disabled(int p_idx, bool p_disabled);
+	_FORCE_INLINE_ bool is_shape_disabled(int p_idx) const {
+		ERR_FAIL_INDEX_V(p_idx, shapes.size(), false);
 		return shapes[p_idx].disabled;
 	}
 
diff --git a/servers/physics_2d/physics_server_2d_sw.cpp b/servers/physics_2d/physics_server_2d_sw.cpp
index 1c2dca0259..155dda4394 100644
--- a/servers/physics_2d/physics_server_2d_sw.cpp
+++ b/servers/physics_2d/physics_server_2d_sw.cpp
@@ -366,7 +366,7 @@ void PhysicsServer2DSW::area_set_shape_disabled(RID p_area, int p_shape, bool p_
 	ERR_FAIL_INDEX(p_shape, area->get_shape_count());
 	FLUSH_QUERY_CHECK(area);
 
-	area->set_shape_as_disabled(p_shape, p_disabled);
+	area->set_shape_disabled(p_shape, p_disabled);
 }
 
 int PhysicsServer2DSW::area_get_shape_count(RID p_area) const {
@@ -663,7 +663,7 @@ void PhysicsServer2DSW::body_set_shape_disabled(RID p_body, int p_shape_idx, boo
 	ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count());
 	FLUSH_QUERY_CHECK(body);
 
-	body->set_shape_as_disabled(p_shape_idx, p_disabled);
+	body->set_shape_disabled(p_shape_idx, p_disabled);
 }
 
 void PhysicsServer2DSW::body_set_shape_as_one_way_collision(RID p_body, int p_shape_idx, bool p_enable, real_t p_margin) {
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index 905f08011b..4b123b2d46 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)) {
@@ -365,10 +353,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 +444,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 +496,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 +518,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 +570,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;
 				}
 
@@ -732,7 +710,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 +773,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;
 				}
 
@@ -918,7 +896,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;
 			}
 
@@ -1060,7 +1038,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;
 			}
 
diff --git a/servers/physics_3d/area_pair_3d_sw.cpp b/servers/physics_3d/area_pair_3d_sw.cpp
index 2073df7dee..9f93a61909 100644
--- a/servers/physics_3d/area_pair_3d_sw.cpp
+++ b/servers/physics_3d/area_pair_3d_sw.cpp
@@ -33,10 +33,7 @@
 
 bool AreaPair3DSW::setup(real_t p_step) {
 	bool result = false;
-
-	if (area->is_shape_set_as_disabled(area_shape) || body->is_shape_set_as_disabled(body_shape)) {
-		result = false;
-	} else if (area->test_collision_mask(body) && CollisionSolver3DSW::solve_static(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), nullptr, this)) {
+	if (area->test_collision_mask(body) && CollisionSolver3DSW::solve_static(body->get_shape(body_shape), body->get_transform() * body->get_shape_transform(body_shape), area->get_shape(area_shape), area->get_transform() * area->get_shape_transform(area_shape), nullptr, this)) {
 		result = true;
 	}
 
@@ -113,9 +110,7 @@ AreaPair3DSW::~AreaPair3DSW() {
 
 bool Area2Pair3DSW::setup(real_t p_step) {
 	bool result = false;
-	if (area_a->is_shape_set_as_disabled(shape_a) || area_b->is_shape_set_as_disabled(shape_b)) {
-		result = false;
-	} else if (area_a->test_collision_mask(area_b) && CollisionSolver3DSW::solve_static(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), nullptr, this)) {
+	if (area_a->test_collision_mask(area_b) && CollisionSolver3DSW::solve_static(area_a->get_shape(shape_a), area_a->get_transform() * area_a->get_shape_transform(shape_a), area_b->get_shape(shape_b), area_b->get_transform() * area_b->get_shape_transform(shape_b), nullptr, this)) {
 		result = true;
 	}
 
diff --git a/servers/physics_3d/body_pair_3d_sw.cpp b/servers/physics_3d/body_pair_3d_sw.cpp
index aed4815c5e..6fc0fa8690 100644
--- a/servers/physics_3d/body_pair_3d_sw.cpp
+++ b/servers/physics_3d/body_pair_3d_sw.cpp
@@ -230,11 +230,6 @@ bool BodyPair3DSW::setup(real_t p_step) {
 		}
 	}
 
-	if (A->is_shape_set_as_disabled(shape_A) || B->is_shape_set_as_disabled(shape_B)) {
-		collided = false;
-		return false;
-	}
-
 	offset_B = B->get_transform().get_origin() - A->get_transform().get_origin();
 
 	validate_contacts();
@@ -607,11 +602,6 @@ bool BodySoftBodyPair3DSW::setup(real_t p_step) {
 		return false;
 	}
 
-	if (body->is_shape_set_as_disabled(body_shape)) {
-		collided = false;
-		return false;
-	}
-
 	const Transform3D &xform_Au = body->get_transform();
 	Transform3D xform_A = xform_Au * body->get_shape_transform(body_shape);
 
diff --git a/servers/physics_3d/collision_object_3d_sw.cpp b/servers/physics_3d/collision_object_3d_sw.cpp
index 51e2432071..24c7d7b85c 100644
--- a/servers/physics_3d/collision_object_3d_sw.cpp
+++ b/servers/physics_3d/collision_object_3d_sw.cpp
@@ -45,8 +45,6 @@ void CollisionObject3DSW::add_shape(Shape3DSW *p_shape, const Transform3D &p_tra
 	if (!pending_shape_update_list.in_list()) {
 		PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list);
 	}
-	//_update_shapes();
-	//_shapes_changed();
 }
 
 void CollisionObject3DSW::set_shape(int p_index, Shape3DSW *p_shape) {
@@ -58,8 +56,6 @@ void CollisionObject3DSW::set_shape(int p_index, Shape3DSW *p_shape) {
 	if (!pending_shape_update_list.in_list()) {
 		PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list);
 	}
-	//_update_shapes();
-	//_shapes_changed();
 }
 
 void CollisionObject3DSW::set_shape_transform(int p_index, const Transform3D &p_transform) {
@@ -70,14 +66,32 @@ void CollisionObject3DSW::set_shape_transform(int p_index, const Transform3D &p_
 	if (!pending_shape_update_list.in_list()) {
 		PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list);
 	}
-	//_update_shapes();
-	//_shapes_changed();
 }
 
-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::singletonsw->pending_shape_update_list.add(&pending_shape_update_list);
+void CollisionObject3DSW::set_shape_disabled(int p_idx, bool p_disabled) {
+	ERR_FAIL_INDEX(p_idx, shapes.size());
+
+	CollisionObject3DSW::Shape &shape = shapes.write[p_idx];
+	if (shape.disabled == p_disabled) {
+		return;
+	}
+
+	shape.disabled = p_disabled;
+
+	if (!space) {
+		return;
+	}
+
+	if (p_disabled && shape.bpid != 0) {
+		space->get_broadphase()->remove(shape.bpid);
+		shape.bpid = 0;
+		if (!pending_shape_update_list.in_list()) {
+			PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list);
+		}
+	} else if (!p_disabled && shape.bpid == 0) {
+		if (!pending_shape_update_list.in_list()) {
+			PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list);
+		}
 	}
 }
 
@@ -108,8 +122,6 @@ void CollisionObject3DSW::remove_shape(int p_index) {
 	if (!pending_shape_update_list.in_list()) {
 		PhysicsServer3DSW::singletonsw->pending_shape_update_list.add(&pending_shape_update_list);
 	}
-	//_update_shapes();
-	//_shapes_changed();
 }
 
 void CollisionObject3DSW::_set_static(bool p_static) {
@@ -146,6 +158,9 @@ void CollisionObject3DSW::_update_shapes() {
 
 	for (int i = 0; i < shapes.size(); i++) {
 		Shape &s = shapes.write[i];
+		if (s.disabled) {
+			continue;
+		}
 
 		//not quite correct, should compute the next matrix..
 		AABB shape_aabb = s.shape->get_aabb();
@@ -173,6 +188,9 @@ void CollisionObject3DSW::_update_shapes_with_motion(const Vector3 &p_motion) {
 
 	for (int i = 0; i < shapes.size(); i++) {
 		Shape &s = shapes.write[i];
+		if (s.disabled) {
+			continue;
+		}
 
 		//not quite correct, should compute the next matrix..
 		AABB shape_aabb = s.shape->get_aabb();
diff --git a/servers/physics_3d/collision_object_3d_sw.h b/servers/physics_3d/collision_object_3d_sw.h
index 5505fec3da..b2b2fa0f50 100644
--- a/servers/physics_3d/collision_object_3d_sw.h
+++ b/servers/physics_3d/collision_object_3d_sw.h
@@ -120,15 +120,26 @@ public:
 	void set_shape(int p_index, Shape3DSW *p_shape);
 	void set_shape_transform(int p_index, const Transform3D &p_transform);
 	_FORCE_INLINE_ int get_shape_count() const { return shapes.size(); }
-	_FORCE_INLINE_ bool is_shape_disabled(int p_index) const {
+	_FORCE_INLINE_ Shape3DSW *get_shape(int p_index) const {
 		CRASH_BAD_INDEX(p_index, shapes.size());
-		return shapes[p_index].disabled;
+		return shapes[p_index].shape;
+	}
+	_FORCE_INLINE_ const Transform3D &get_shape_transform(int p_index) const {
+		CRASH_BAD_INDEX(p_index, shapes.size());
+		return shapes[p_index].xform;
+	}
+	_FORCE_INLINE_ const Transform3D &get_shape_inv_transform(int p_index) const {
+		CRASH_BAD_INDEX(p_index, shapes.size());
+		return shapes[p_index].xform_inv;
+	}
+	_FORCE_INLINE_ const AABB &get_shape_aabb(int p_index) const {
+		CRASH_BAD_INDEX(p_index, shapes.size());
+		return shapes[p_index].aabb_cache;
+	}
+	_FORCE_INLINE_ real_t get_shape_area(int p_index) const {
+		CRASH_BAD_INDEX(p_index, shapes.size());
+		return shapes[p_index].area_cache;
 	}
-	_FORCE_INLINE_ Shape3DSW *get_shape(int p_index) const { return shapes[p_index].shape; }
-	_FORCE_INLINE_ const Transform3D &get_shape_transform(int p_index) const { return shapes[p_index].xform; }
-	_FORCE_INLINE_ const Transform3D &get_shape_inv_transform(int p_index) const { return shapes[p_index].xform_inv; }
-	_FORCE_INLINE_ const AABB &get_shape_aabb(int p_index) const { return shapes[p_index].aabb_cache; }
-	_FORCE_INLINE_ real_t get_shape_area(int p_index) const { return shapes[p_index].area_cache; }
 
 	_FORCE_INLINE_ const Transform3D &get_transform() const { return transform; }
 	_FORCE_INLINE_ const Transform3D &get_inv_transform() const { return inv_transform; }
@@ -137,9 +148,9 @@ public:
 	_FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable = p_enable; }
 	_FORCE_INLINE_ bool is_ray_pickable() const { return ray_pickable; }
 
-	void set_shape_as_disabled(int p_idx, bool p_enable);
-	_FORCE_INLINE_ bool is_shape_set_as_disabled(int p_idx) const {
-		CRASH_BAD_INDEX(p_idx, shapes.size());
+	void set_shape_disabled(int p_idx, bool p_disabled);
+	_FORCE_INLINE_ bool is_shape_disabled(int p_idx) const {
+		ERR_FAIL_INDEX_V(p_idx, shapes.size(), false);
 		return shapes[p_idx].disabled;
 	}
 
diff --git a/servers/physics_3d/physics_server_3d_sw.cpp b/servers/physics_3d/physics_server_3d_sw.cpp
index 7a95a8abc8..f26129a404 100644
--- a/servers/physics_3d/physics_server_3d_sw.cpp
+++ b/servers/physics_3d/physics_server_3d_sw.cpp
@@ -335,7 +335,7 @@ void PhysicsServer3DSW::area_set_shape_disabled(RID p_area, int p_shape_idx, boo
 	ERR_FAIL_COND(!area);
 	ERR_FAIL_INDEX(p_shape_idx, area->get_shape_count());
 	FLUSH_QUERY_CHECK(area);
-	area->set_shape_as_disabled(p_shape_idx, p_disabled);
+	area->set_shape_disabled(p_shape_idx, p_disabled);
 }
 
 void PhysicsServer3DSW::area_attach_object_instance_id(RID p_area, ObjectID p_id) {
@@ -537,7 +537,7 @@ void PhysicsServer3DSW::body_set_shape_disabled(RID p_body, int p_shape_idx, boo
 	ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count());
 	FLUSH_QUERY_CHECK(body);
 
-	body->set_shape_as_disabled(p_shape_idx, p_disabled);
+	body->set_shape_disabled(p_shape_idx, p_disabled);
 }
 
 Transform3D PhysicsServer3DSW::body_get_shape_transform(RID p_body, int p_shape_idx) const {
diff --git a/servers/physics_3d/space_3d_sw.cpp b/servers/physics_3d/space_3d_sw.cpp
index e411ff8266..eff480396d 100644
--- a/servers/physics_3d/space_3d_sw.cpp
+++ b/servers/physics_3d/space_3d_sw.cpp
@@ -214,10 +214,6 @@ int PhysicsDirectSpaceState3DSW::intersect_shape(const RID &p_shape, const Trans
 		const CollisionObject3DSW *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 (!CollisionSolver3DSW::solve_static(shape, p_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), nullptr, nullptr, nullptr, p_margin, 0)) {
 			continue;
 		}
@@ -273,10 +269,6 @@ bool PhysicsDirectSpaceState3DSW::cast_motion(const RID &p_shape, const Transfor
 		const CollisionObject3DSW *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;
-		}
-
 		Vector3 point_A, point_B;
 		Vector3 sep_axis = p_motion.normalized();
 
@@ -385,10 +377,6 @@ bool PhysicsDirectSpaceState3DSW::collide_shape(RID p_shape, const Transform3D &
 
 		int shape_idx = space->intersection_query_subindex_results[i];
 
-		if (col_obj->is_shape_set_as_disabled(shape_idx)) {
-			continue;
-		}
-
 		if (CollisionSolver3DSW::solve_static(shape, p_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 = true;
 		}
@@ -460,10 +448,6 @@ bool PhysicsDirectSpaceState3DSW::rest_info(RID p_shape, const Transform3D &p_sh
 
 		int shape_idx = space->intersection_query_subindex_results[i];
 
-		if (col_obj->is_shape_set_as_disabled(shape_idx)) {
-			continue;
-		}
-
 		rcd.object = col_obj;
 		rcd.shape = shape_idx;
 		bool sc = CollisionSolver3DSW::solve_static(shape, p_shape_xform, col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), _rest_cbk_result, &rcd, nullptr, p_margin);
@@ -508,7 +492,7 @@ Vector3 PhysicsDirectSpaceState3DSW::get_closest_point_to_object_volume(RID p_ob
 	bool shapes_found = false;
 
 	for (int i = 0; i < obj->get_shape_count(); i++) {
-		if (obj->is_shape_set_as_disabled(i)) {
+		if (obj->is_shape_disabled(i)) {
 			continue;
 		}
 
@@ -555,8 +539,6 @@ int Space3DSW::_cull_aabb_for_body(Body3DSW *p_body, const AABB &p_aabb) {
 			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;
-		} else if (static_cast<Body3DSW *>(intersection_query_results[i])->is_shape_set_as_disabled(intersection_query_subindex_results[i])) {
-			keep = false;
 		}
 
 		if (!keep) {
@@ -579,7 +561,7 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform3D &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;
 		}
 
@@ -626,7 +608,7 @@ int Space3DSW::test_body_ray_separation(Body3DSW *p_body, const Transform3D &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;
 				}
 
@@ -740,7 +722,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &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;
 		}
 
@@ -793,7 +775,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &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;
 				}
 
@@ -871,7 +853,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &p_from, co
 		int amount = _cull_aabb_for_body(p_body, motion_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;
 			}
 
@@ -989,7 +971,7 @@ bool Space3DSW::test_body_motion(Body3DSW *p_body, const Transform3D &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;
 			}