1 files changed, 67 insertions, 2 deletions

diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp
index f2dbb635f8..472283833e 100644
--- a/servers/physics/physics_server_sw.cpp
+++ b/servers/physics/physics_server_sw.cpp
@@ -65,6 +65,11 @@ RID PhysicsServerSW::shape_create(ShapeType p_shape) {
 
 			shape = memnew(CapsuleShapeSW);
 		} break;
+		case SHAPE_CYLINDER: {
+
+			ERR_EXPLAIN("CylinderShape is not supported in GodotPhysics. Please switch to Bullet in the Project Settings.");
+			ERR_FAIL_V(RID());
+		} break;
 		case SHAPE_CONVEX_POLYGON: {
 
 			shape = memnew(ConvexPolygonShapeSW);
@@ -119,6 +124,13 @@ Variant PhysicsServerSW::shape_get_data(RID p_shape) const {
 	return shape->get_data();
 };
 
+void PhysicsServerSW::shape_set_margin(RID p_shape, real_t p_margin) {
+}
+
+real_t PhysicsServerSW::shape_get_margin(RID p_shape) const {
+	return 0.0;
+}
+
 real_t PhysicsServerSW::shape_get_custom_solver_bias(RID p_shape) const {
 
 	const ShapeSW *shape = shape_owner.get(p_shape);
@@ -287,6 +299,7 @@ void PhysicsServerSW::area_set_shape(RID p_area, int p_shape_idx, RID p_shape) {
 
 	area->set_shape(p_shape_idx, shape);
 }
+
 void PhysicsServerSW::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform &p_transform) {
 
 	AreaSW *area = area_owner.get(p_area);
@@ -758,6 +771,40 @@ Vector3 PhysicsServerSW::body_get_applied_torque(RID p_body) const {
 	return body->get_applied_torque();
 };
 
+void PhysicsServerSW::body_add_central_force(RID p_body, const Vector3 &p_force) {
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->add_central_force(p_force);
+	body->wakeup();
+}
+
+void PhysicsServerSW::body_add_force(RID p_body, const Vector3 &p_force, const Vector3 &p_pos) {
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->add_force(p_force, p_pos);
+	body->wakeup();
+};
+
+void PhysicsServerSW::body_add_torque(RID p_body, const Vector3 &p_torque) {
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	body->add_torque(p_torque);
+	body->wakeup();
+};
+
+void PhysicsServerSW::body_apply_central_impulse(RID p_body, const Vector3 &p_impulse) {
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+
+	_update_shapes();
+
+	body->apply_central_impulse(p_impulse);
+	body->wakeup();
+}
+
 void PhysicsServerSW::body_apply_impulse(RID p_body, const Vector3 &p_pos, const Vector3 &p_impulse) {
 
 	BodySW *body = body_owner.get(p_body);
@@ -902,7 +949,7 @@ bool PhysicsServerSW::body_is_ray_pickable(RID p_body) const {
 	return body->is_ray_pickable();
 }
 
-bool PhysicsServerSW::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result) {
+bool PhysicsServerSW::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia, MotionResult *r_result, bool p_exclude_raycast_shapes) {
 
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND_V(!body, false);
@@ -911,7 +958,19 @@ bool PhysicsServerSW::body_test_motion(RID p_body, const Transform &p_from, cons
 
 	_update_shapes();
 
-	return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, body->get_kinematic_margin(), r_result);
+	return body->get_space()->test_body_motion(body, p_from, p_motion, p_infinite_inertia, body->get_kinematic_margin(), r_result, p_exclude_raycast_shapes);
+}
+
+int PhysicsServerSW::body_test_ray_separation(RID p_body, const Transform &p_transform, bool p_infinite_inertia, Vector3 &r_recover_motion, SeparationResult *r_results, int p_result_max, float p_margin) {
+
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, false);
+	ERR_FAIL_COND_V(!body->get_space(), false);
+	ERR_FAIL_COND_V(body->get_space()->is_locked(), false);
+
+	_update_shapes();
+
+	return body->get_space()->test_body_ray_separation(body, p_transform, p_infinite_inertia, r_recover_motion, r_results, p_result_max, p_margin);
 }
 
 PhysicsDirectBodyState *PhysicsServerSW::body_get_direct_state(RID p_body) {
@@ -1362,6 +1421,8 @@ void PhysicsServerSW::init() {
 
 void PhysicsServerSW::step(real_t p_step) {
 
+#ifndef _3D_DISABLED
+
 	if (!active)
 		return;
 
@@ -1382,6 +1443,7 @@ void PhysicsServerSW::step(real_t p_step) {
 		active_objects += E->get()->get_active_objects();
 		collision_pairs += E->get()->get_collision_pairs();
 	}
+#endif
 }
 
 void PhysicsServerSW::sync(){
@@ -1390,6 +1452,8 @@ void PhysicsServerSW::sync(){
 
 void PhysicsServerSW::flush_queries() {
 
+#ifndef _3D_DISABLED
+
 	if (!active)
 		return;
 
@@ -1436,6 +1500,7 @@ void PhysicsServerSW::flush_queries() {
 
 		ScriptDebugger::get_singleton()->add_profiling_frame_data("physics", values);
 	}
+#endif
 };
 
 void PhysicsServerSW::finish() {