5 files changed, 52 insertions, 6 deletions

diff --git a/servers/physics/body_sw.h b/servers/physics/body_sw.h
index e8ea5531e5..fd2ab16b84 100644
--- a/servers/physics/body_sw.h
+++ b/servers/physics/body_sw.h
@@ -245,12 +245,21 @@ public:
 		biased_angular_velocity += _inv_inertia_tensor.xform(p_j);
 	}
 
+	_FORCE_INLINE_ void add_central_force(const Vector3 &p_force) {
+
+		applied_force += p_force;
+	}
+
 	_FORCE_INLINE_ void add_force(const Vector3 &p_force, const Vector3 &p_pos) {
 
 		applied_force += p_force;
 		applied_torque += p_pos.cross(p_force);
 	}
 
+	_FORCE_INLINE_ void add_torque(const Vector3 &p_torque) {
+		applied_torque += p_torque;
+	}
+
 	void set_active(bool p_active);
 	_FORCE_INLINE_ bool is_active() const { return active; }
 
@@ -401,7 +410,9 @@ public:
 	virtual void set_transform(const Transform &p_transform) { body->set_state(PhysicsServer::BODY_STATE_TRANSFORM, p_transform); }
 	virtual Transform get_transform() const { return body->get_transform(); }
 
+	virtual void add_central_force(const Vector3 &p_force) { body->add_central_force(p_force); }
 	virtual void add_force(const Vector3 &p_force, const Vector3 &p_pos) { body->add_force(p_force, p_pos); }
+	virtual void add_torque(const Vector3 &p_torque) { body->add_torque(p_torque); }
 	virtual void apply_impulse(const Vector3 &p_pos, const Vector3 &p_j) { body->apply_impulse(p_pos, p_j); }
 	virtual void apply_torque_impulse(const Vector3 &p_j) { body->apply_torque_impulse(p_j); }
 
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 5681ca838a..0f7c6deaac 100644
--- a/servers/physics/physics_server_sw.cpp
+++ b/servers/physics/physics_server_sw.cpp
@@ -795,12 +795,12 @@ void PhysicsServerSW::body_set_axis_velocity(RID p_body, const Vector3 &p_axis_v
 	body->wakeup();
 };
 
-void PhysicsServerSW::body_set_axis_lock(RID p_body, BodyAxis p_axis, bool lock) {
+void PhysicsServerSW::body_set_axis_lock(RID p_body, BodyAxis p_axis, bool p_lock) {
 
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
-	body->set_axis_lock(p_axis, lock);
+	body->set_axis_lock(p_axis, p_lock);
 	body->wakeup();
 }
 
@@ -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/shape_sw.cpp b/servers/physics/shape_sw.cpp
index 5a58742958..b918a42ca5 100644
--- a/servers/physics/shape_sw.cpp
+++ b/servers/physics/shape_sw.cpp
@@ -672,7 +672,7 @@ Vector3 CapsuleShapeSW::get_closest_point_to(const Vector3 &p_point) const {
 
 Vector3 CapsuleShapeSW::get_moment_of_inertia(real_t p_mass) const {
 
-	// use crappy AABB approximation
+	// use bad AABB approximation
 	Vector3 extents = get_aabb().size * 0.5;
 
 	return Vector3(
@@ -943,7 +943,7 @@ Vector3 ConvexPolygonShapeSW::get_closest_point_to(const Vector3 &p_point) const
 
 Vector3 ConvexPolygonShapeSW::get_moment_of_inertia(real_t p_mass) const {
 
-	// use crappy AABB approximation
+	// use bad AABB approximation
 	Vector3 extents = get_aabb().size * 0.5;
 
 	return Vector3(
@@ -1331,7 +1331,7 @@ void ConcavePolygonShapeSW::cull(const AABB &p_local_aabb, Callback p_callback,
 
 Vector3 ConcavePolygonShapeSW::get_moment_of_inertia(real_t p_mass) const {
 
-	// use crappy AABB approximation
+	// use bad AABB approximation
 	Vector3 extents = get_aabb().size * 0.5;
 
 	return Vector3(
@@ -1594,7 +1594,7 @@ void HeightMapShapeSW::cull(const AABB &p_local_aabb, Callback p_callback, void
 
 Vector3 HeightMapShapeSW::get_moment_of_inertia(real_t p_mass) const {
 
-	// use crappy AABB approximation
+	// use bad AABB approximation
 	Vector3 extents = get_aabb().size * 0.5;
 
 	return Vector3(