Added physics API in order to enable/disable collisions between rigidbody attached to a joint with bullet physics bullet

Fixes #16424

15 files changed, 121 insertions, 13 deletions

diff --git a/modules/bullet/bullet_physics_server.cpp b/modules/bullet/bullet_physics_server.cpp
index 51de4998fa..b646fc164d 100644
--- a/modules/bullet/bullet_physics_server.cpp
+++ b/modules/bullet/bullet_physics_server.cpp
@@ -70,8 +70,8 @@
 		return RID();                                                                             \
 	}
 
-#define AddJointToSpace(body, joint, disableCollisionsBetweenLinkedBodies) \
-	body->get_space()->add_constraint(joint, disableCollisionsBetweenLinkedBodies);
+#define AddJointToSpace(body, joint) \
+	body->get_space()->add_constraint(joint, joint->is_disabled_collisions_between_bodies());
 // <--------------- Joint creation asserts
 
 btEmptyShape *BulletPhysicsServer::emptyShape(ShapeBullet::create_shape_empty());
@@ -987,6 +987,20 @@ int BulletPhysicsServer::joint_get_solver_priority(RID p_joint) const {
 	return 0;
 }
 
+void BulletPhysicsServer::joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable) {
+	JointBullet *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND(!joint);
+
+	joint->disable_collisions_between_bodies(p_disable);
+}
+
+bool BulletPhysicsServer::joint_is_disabled_collisions_between_bodies(RID p_joint) const {
+	JointBullet *joint(joint_owner.get(p_joint));
+	ERR_FAIL_COND_V(!joint, false);
+
+	return joint->is_disabled_collisions_between_bodies();
+}
+
 RID BulletPhysicsServer::joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) {
 	RigidBodyBullet *body_A = rigid_body_owner.get(p_body_A);
 	ERR_FAIL_COND_V(!body_A, RID());
@@ -1003,7 +1017,7 @@ RID BulletPhysicsServer::joint_create_pin(RID p_body_A, const Vector3 &p_local_A
 	ERR_FAIL_COND_V(body_A == body_B, RID());
 
 	JointBullet *joint = bulletnew(PinJointBullet(body_A, p_local_A, body_B, p_local_B));
-	AddJointToSpace(body_A, joint, true);
+	AddJointToSpace(body_A, joint);
 
 	CreateThenReturnRID(joint_owner, joint);
 }
@@ -1071,7 +1085,7 @@ RID BulletPhysicsServer::joint_create_hinge(RID p_body_A, const Transform &p_hin
 	ERR_FAIL_COND_V(body_A == body_B, RID());
 
 	JointBullet *joint = bulletnew(HingeJointBullet(body_A, body_B, p_hinge_A, p_hinge_B));
-	AddJointToSpace(body_A, joint, true);
+	AddJointToSpace(body_A, joint);
 
 	CreateThenReturnRID(joint_owner, joint);
 }
@@ -1091,7 +1105,7 @@ RID BulletPhysicsServer::joint_create_hinge_simple(RID p_body_A, const Vector3 &
 	ERR_FAIL_COND_V(body_A == body_B, RID());
 
 	JointBullet *joint = bulletnew(HingeJointBullet(body_A, body_B, p_pivot_A, p_pivot_B, p_axis_A, p_axis_B));
-	AddJointToSpace(body_A, joint, true);
+	AddJointToSpace(body_A, joint);
 
 	CreateThenReturnRID(joint_owner, joint);
 }
@@ -1143,7 +1157,7 @@ RID BulletPhysicsServer::joint_create_slider(RID p_body_A, const Transform &p_lo
 	ERR_FAIL_COND_V(body_A == body_B, RID());
 
 	JointBullet *joint = bulletnew(SliderJointBullet(body_A, body_B, p_local_frame_A, p_local_frame_B));
-	AddJointToSpace(body_A, joint, true);
+	AddJointToSpace(body_A, joint);
 
 	CreateThenReturnRID(joint_owner, joint);
 }
@@ -1177,7 +1191,7 @@ RID BulletPhysicsServer::joint_create_cone_twist(RID p_body_A, const Transform &
 	}
 
 	JointBullet *joint = bulletnew(ConeTwistJointBullet(body_A, body_B, p_local_frame_A, p_local_frame_B));
-	AddJointToSpace(body_A, joint, true);
+	AddJointToSpace(body_A, joint);
 
 	CreateThenReturnRID(joint_owner, joint);
 }
@@ -1213,7 +1227,7 @@ RID BulletPhysicsServer::joint_create_generic_6dof(RID p_body_A, const Transform
 	ERR_FAIL_COND_V(body_A == body_B, RID());
 
 	JointBullet *joint = bulletnew(Generic6DOFJointBullet(body_A, body_B, p_local_frame_A, p_local_frame_B, true));
-	AddJointToSpace(body_A, joint, true);
+	AddJointToSpace(body_A, joint);
 
 	CreateThenReturnRID(joint_owner, joint);
 }
diff --git a/modules/bullet/bullet_physics_server.h b/modules/bullet/bullet_physics_server.h
index e0e46cd369..764ec2387c 100644
--- a/modules/bullet/bullet_physics_server.h
+++ b/modules/bullet/bullet_physics_server.h
@@ -290,6 +290,9 @@ public:
 	virtual void joint_set_solver_priority(RID p_joint, int p_priority);
 	virtual int joint_get_solver_priority(RID p_joint) const;
 
+	virtual void joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable);
+	virtual bool joint_is_disabled_collisions_between_bodies(RID p_joint) const;
+
 	virtual RID joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B);
 
 	virtual void pin_joint_set_param(RID p_joint, PinJointParam p_param, float p_value);
diff --git a/modules/bullet/constraint_bullet.cpp b/modules/bullet/constraint_bullet.cpp
index b60e89b6fd..d15fb8de01 100644
--- a/modules/bullet/constraint_bullet.cpp
+++ b/modules/bullet/constraint_bullet.cpp
@@ -39,7 +39,8 @@
 
 ConstraintBullet::ConstraintBullet() :
 		space(NULL),
-		constraint(NULL) {}
+		constraint(NULL),
+		disabled_collisions_between_bodies(true) {}
 
 void ConstraintBullet::setup(btTypedConstraint *p_constraint) {
 	constraint = p_constraint;
@@ -53,3 +54,12 @@ void ConstraintBullet::set_space(SpaceBullet *p_space) {
 void ConstraintBullet::destroy_internal_constraint() {
 	space->remove_constraint(this);
 }
+
+void ConstraintBullet::disable_collisions_between_bodies(const bool p_disabled) {
+	disabled_collisions_between_bodies = p_disabled;
+
+	if (space) {
+		space->remove_constraint(this);
+		space->add_constraint(this, disabled_collisions_between_bodies);
+	}
+}
diff --git a/modules/bullet/constraint_bullet.h b/modules/bullet/constraint_bullet.h
index 23be5a5063..ed3a318cbc 100644
--- a/modules/bullet/constraint_bullet.h
+++ b/modules/bullet/constraint_bullet.h
@@ -49,6 +49,7 @@ class ConstraintBullet : public RIDBullet {
 protected:
 	SpaceBullet *space;
 	btTypedConstraint *constraint;
+	bool disabled_collisions_between_bodies;
 
 public:
 	ConstraintBullet();
@@ -57,6 +58,9 @@ public:
 	virtual void set_space(SpaceBullet *p_space);
 	virtual void destroy_internal_constraint();
 
+	void disable_collisions_between_bodies(const bool p_disabled);
+	_FORCE_INLINE_ bool is_disabled_collisions_between_bodies() const { return disabled_collisions_between_bodies; }
+
 public:
 	virtual ~ConstraintBullet() {
 		bulletdelete(constraint);
diff --git a/scene/2d/joints_2d.cpp b/scene/2d/joints_2d.cpp
index 7a96a54854..329382c034 100644
--- a/scene/2d/joints_2d.cpp
+++ b/scene/2d/joints_2d.cpp
@@ -75,8 +75,7 @@ void Joint2D::_update_joint(bool p_only_free) {
 	ba = body_a->get_rid();
 	bb = body_b->get_rid();
 
-	if (exclude_from_collision)
-		Physics2DServer::get_singleton()->body_add_collision_exception(body_a->get_rid(), body_b->get_rid());
+	Physics2DServer::get_singleton()->joint_disable_collisions_between_bodies(joint, exclude_from_collision);
 }
 
 void Joint2D::set_node_a(const NodePath &p_node_a) {
diff --git a/scene/3d/physics_joint.cpp b/scene/3d/physics_joint.cpp
index fed6d76f65..2e9f1a241a 100644
--- a/scene/3d/physics_joint.cpp
+++ b/scene/3d/physics_joint.cpp
@@ -71,8 +71,7 @@ void Joint::_update_joint(bool p_only_free) {
 	ba = body_a->get_rid();
 	bb = body_b->get_rid();
 
-	if (exclude_from_collision)
-		PhysicsServer::get_singleton()->body_add_collision_exception(body_a->get_rid(), body_b->get_rid());
+	PhysicsServer::get_singleton()->joint_disable_collisions_between_bodies(joint, exclude_from_collision);
 }
 
 void Joint::set_node_a(const NodePath &p_node_a) {
diff --git a/servers/physics/constraint_sw.h b/servers/physics/constraint_sw.h
index a641f06f0c..41789600f6 100644
--- a/servers/physics/constraint_sw.h
+++ b/servers/physics/constraint_sw.h
@@ -41,6 +41,7 @@ class ConstraintSW : public RID_Data {
 	ConstraintSW *island_next;
 	ConstraintSW *island_list_next;
 	int priority;
+	bool disabled_collisions_between_bodies;
 
 	RID self;
 
@@ -50,6 +51,7 @@ protected:
 		_body_count = p_body_count;
 		island_step = 0;
 		priority = 1;
+		disabled_collisions_between_bodies = true;
 	}
 
 public:
@@ -71,6 +73,9 @@ public:
 	_FORCE_INLINE_ void set_priority(int p_priority) { priority = p_priority; }
 	_FORCE_INLINE_ int get_priority() const { return priority; }
 
+	_FORCE_INLINE_ void disable_collisions_between_bodies(const bool p_disabled) { disabled_collisions_between_bodies = p_disabled; }
+	_FORCE_INLINE_ bool is_disabled_collisions_between_bodies() const { return disabled_collisions_between_bodies; }
+
 	virtual bool setup(real_t p_step) = 0;
 	virtual void solve(real_t p_step) = 0;
 
diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp
index ea0d372281..0f7c6deaac 100644
--- a/servers/physics/physics_server_sw.cpp
+++ b/servers/physics/physics_server_sw.cpp
@@ -1093,6 +1093,33 @@ int PhysicsServerSW::joint_get_solver_priority(RID p_joint) const {
 	return joint->get_priority();
 }
 
+void PhysicsServerSW::joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable) {
+	JointSW *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND(!joint);
+
+	joint->disable_collisions_between_bodies(p_disable);
+
+	if (2 == joint->get_body_count()) {
+		BodySW *body_a = *joint->get_body_ptr();
+		BodySW *body_b = *(joint->get_body_ptr() + 1);
+
+		if (p_disable) {
+			body_add_collision_exception(body_a->get_self(), body_b->get_self());
+			body_add_collision_exception(body_b->get_self(), body_a->get_self());
+		} else {
+			body_remove_collision_exception(body_a->get_self(), body_b->get_self());
+			body_remove_collision_exception(body_b->get_self(), body_a->get_self());
+		}
+	}
+}
+
+bool PhysicsServerSW::joint_is_disabled_collisions_between_bodies(RID p_joint) const {
+	JointSW *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND_V(!joint, true);
+
+	return joint->is_disabled_collisions_between_bodies();
+}
+
 PhysicsServerSW::JointType PhysicsServerSW::joint_get_type(RID p_joint) const {
 
 	JointSW *joint = joint_owner.get(p_joint);
diff --git a/servers/physics/physics_server_sw.h b/servers/physics/physics_server_sw.h
index 132ac78968..923b59d28f 100644
--- a/servers/physics/physics_server_sw.h
+++ b/servers/physics/physics_server_sw.h
@@ -275,6 +275,9 @@ public:
 	virtual void joint_set_solver_priority(RID p_joint, int p_priority);
 	virtual int joint_get_solver_priority(RID p_joint) const;
 
+	virtual void joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable);
+	virtual bool joint_is_disabled_collisions_between_bodies(RID p_joint) const;
+
 	/* MISC */
 
 	virtual void free(RID p_rid);
diff --git a/servers/physics_2d/constraint_2d_sw.h b/servers/physics_2d/constraint_2d_sw.h
index a08037bb37..c1954935d3 100644
--- a/servers/physics_2d/constraint_2d_sw.h
+++ b/servers/physics_2d/constraint_2d_sw.h
@@ -40,6 +40,7 @@ class Constraint2DSW : public RID_Data {
 	uint64_t island_step;
 	Constraint2DSW *island_next;
 	Constraint2DSW *island_list_next;
+	bool disabled_collisions_between_bodies;
 
 	RID self;
 
@@ -48,6 +49,7 @@ protected:
 		_body_ptr = p_body_ptr;
 		_body_count = p_body_count;
 		island_step = 0;
+		disabled_collisions_between_bodies = true;
 	}
 
 public:
@@ -66,6 +68,9 @@ public:
 	_FORCE_INLINE_ Body2DSW **get_body_ptr() const { return _body_ptr; }
 	_FORCE_INLINE_ int get_body_count() const { return _body_count; }
 
+	_FORCE_INLINE_ void disable_collisions_between_bodies(const bool p_disabled) { disabled_collisions_between_bodies = p_disabled; }
+	_FORCE_INLINE_ bool is_disabled_collisions_between_bodies() const { return disabled_collisions_between_bodies; }
+
 	virtual bool setup(real_t p_step) = 0;
 	virtual void solve(real_t p_step) = 0;
 
diff --git a/servers/physics_2d/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp
index 7d7bbbebac..0603287a79 100644
--- a/servers/physics_2d/physics_2d_server_sw.cpp
+++ b/servers/physics_2d/physics_2d_server_sw.cpp
@@ -1015,6 +1015,33 @@ real_t Physics2DServerSW::joint_get_param(RID p_joint, JointParam p_param) const
 	return 0;
 }
 
+void Physics2DServerSW::joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable) {
+	Joint2DSW *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND(!joint);
+
+	joint->disable_collisions_between_bodies(p_disable);
+
+	if (2 == joint->get_body_count()) {
+		Body2DSW *body_a = *joint->get_body_ptr();
+		Body2DSW *body_b = *(joint->get_body_ptr() + 1);
+
+		if (p_disable) {
+			body_add_collision_exception(body_a->get_self(), body_b->get_self());
+			body_add_collision_exception(body_b->get_self(), body_a->get_self());
+		} else {
+			body_remove_collision_exception(body_a->get_self(), body_b->get_self());
+			body_remove_collision_exception(body_b->get_self(), body_a->get_self());
+		}
+	}
+}
+
+bool Physics2DServerSW::joint_is_disabled_collisions_between_bodies(RID p_joint) const {
+	const Joint2DSW *joint = joint_owner.get(p_joint);
+	ERR_FAIL_COND_V(!joint, true);
+
+	return joint->is_disabled_collisions_between_bodies();
+}
+
 RID Physics2DServerSW::pin_joint_create(const Vector2 &p_pos, RID p_body_a, RID p_body_b) {
 
 	Body2DSW *A = body_owner.get(p_body_a);
diff --git a/servers/physics_2d/physics_2d_server_sw.h b/servers/physics_2d/physics_2d_server_sw.h
index 97edb85582..cf9c2957bf 100644
--- a/servers/physics_2d/physics_2d_server_sw.h
+++ b/servers/physics_2d/physics_2d_server_sw.h
@@ -242,6 +242,9 @@ public:
 	virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value);
 	virtual real_t joint_get_param(RID p_joint, JointParam p_param) const;
 
+	virtual void joint_disable_collisions_between_bodies(RID p_joint, const bool p_disabled);
+	virtual bool joint_is_disabled_collisions_between_bodies(RID p_joint) const;
+
 	virtual RID pin_joint_create(const Vector2 &p_pos, RID p_body_a, RID p_body_b = RID());
 	virtual RID groove_joint_create(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RID p_body_a, RID p_body_b);
 	virtual RID damped_spring_joint_create(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RID p_body_a, RID p_body_b = RID());
diff --git a/servers/physics_2d/physics_2d_server_wrap_mt.h b/servers/physics_2d/physics_2d_server_wrap_mt.h
index 276c37c577..d625bc9892 100644
--- a/servers/physics_2d/physics_2d_server_wrap_mt.h
+++ b/servers/physics_2d/physics_2d_server_wrap_mt.h
@@ -263,6 +263,9 @@ public:
 	FUNC3(joint_set_param, RID, JointParam, real_t);
 	FUNC2RC(real_t, joint_get_param, RID, JointParam);
 
+	FUNC2(joint_disable_collisions_between_bodies, RID, const bool);
+	FUNC1RC(bool, joint_is_disabled_collisions_between_bodies, RID);
+
 	///FUNC3RID(pin_joint,const Vector2&,RID,RID);
 	///FUNC5RID(groove_joint,const Vector2&,const Vector2&,const Vector2&,RID,RID);
 	///FUNC4RID(damped_spring_joint,const Vector2&,const Vector2&,RID,RID);
diff --git a/servers/physics_2d_server.h b/servers/physics_2d_server.h
index be447ed137..462244c667 100644
--- a/servers/physics_2d_server.h
+++ b/servers/physics_2d_server.h
@@ -499,6 +499,9 @@ public:
 	virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value) = 0;
 	virtual real_t joint_get_param(RID p_joint, JointParam p_param) const = 0;
 
+	virtual void joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable) = 0;
+	virtual bool joint_is_disabled_collisions_between_bodies(RID p_joint) const = 0;
+
 	virtual RID pin_joint_create(const Vector2 &p_anchor, RID p_body_a, RID p_body_b = RID()) = 0;
 	virtual RID groove_joint_create(const Vector2 &p_a_groove1, const Vector2 &p_a_groove2, const Vector2 &p_b_anchor, RID p_body_a, RID p_body_b) = 0;
 	virtual RID damped_spring_joint_create(const Vector2 &p_anchor_a, const Vector2 &p_anchor_b, RID p_body_a, RID p_body_b = RID()) = 0;
diff --git a/servers/physics_server.h b/servers/physics_server.h
index 94fc8d479d..2ac405293e 100644
--- a/servers/physics_server.h
+++ b/servers/physics_server.h
@@ -491,6 +491,9 @@ public:
 	virtual void joint_set_solver_priority(RID p_joint, int p_priority) = 0;
 	virtual int joint_get_solver_priority(RID p_joint) const = 0;
 
+	virtual void joint_disable_collisions_between_bodies(RID p_joint, const bool p_disable) = 0;
+	virtual bool joint_is_disabled_collisions_between_bodies(RID p_joint) const = 0;
+
 	virtual RID joint_create_pin(RID p_body_A, const Vector3 &p_local_A, RID p_body_B, const Vector3 &p_local_B) = 0;
 
 	enum PinJointParam {