Merge pull request #12713 from AndreaCatania/master

Rewritten kinematic system

4 files changed, 28 insertions, 3 deletions

diff --git a/servers/physics/body_sw.cpp b/servers/physics/body_sw.cpp
index 6ced004118..f8cd6ca858 100644
--- a/servers/physics/body_sw.cpp
+++ b/servers/physics/body_sw.cpp
@@ -736,6 +736,10 @@ void BodySW::set_force_integration_callback(ObjectID p_id, const StringName &p_m
 	}
 }
 
+void BodySW::set_kinematic_margin(real_t p_margin) {
+	kinematic_safe_margin = p_margin;
+}
+
 BodySW::BodySW()
 	: CollisionObjectSW(TYPE_BODY), active_list(this), inertia_update_list(this), direct_state_query_list(this) {
 
@@ -743,6 +747,7 @@ BodySW::BodySW()
 	active = true;
 
 	mass = 1;
+	kinematic_safe_margin = 0.01;
 	//_inv_inertia=Transform();
 	_inv_mass = 1;
 	bounce = 0;
diff --git a/servers/physics/body_sw.h b/servers/physics/body_sw.h
index 98095fd3c6..738d99c764 100644
--- a/servers/physics/body_sw.h
+++ b/servers/physics/body_sw.h
@@ -55,6 +55,7 @@ class BodySW : public CollisionObjectSW {
 
 	PhysicsServer::BodyAxisLock axis_lock;
 
+	real_t kinematic_safe_margin;
 	real_t _inv_mass;
 	Vector3 _inv_inertia; // Relative to the principal axes of inertia
 
@@ -149,6 +150,9 @@ class BodySW : public CollisionObjectSW {
 public:
 	void set_force_integration_callback(ObjectID p_id, const StringName &p_method, const Variant &p_udata = Variant());
 
+	void set_kinematic_margin(real_t p_margin);
+	_FORCE_INLINE_ real_t get_kinematic_margin() { return kinematic_safe_margin; }
+
 	_FORCE_INLINE_ void add_area(AreaSW *p_area) {
 		int index = areas.find(AreaCMP(p_area));
 		if (index > -1) {
diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp
index 5ba935d47c..ce63d84617 100644
--- a/servers/physics/physics_server_sw.cpp
+++ b/servers/physics/physics_server_sw.cpp
@@ -695,6 +695,19 @@ real_t PhysicsServerSW::body_get_param(RID p_body, BodyParameter p_param) const
 	return body->get_param(p_param);
 };
 
+void PhysicsServerSW::body_set_kinematic_safe_margin(RID p_body, real_t p_margin) {
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	body->set_kinematic_margin(p_margin);
+}
+
+real_t PhysicsServerSW::body_get_kinematic_safe_margin(RID p_body) const {
+	BodySW *body = body_owner.get(p_body);
+	ERR_FAIL_COND_V(!body, 0);
+
+	return body->get_kinematic_margin();
+}
+
 void PhysicsServerSW::body_set_state(RID p_body, BodyState p_state, const Variant &p_variant) {
 
 	BodySW *body = body_owner.get(p_body);
@@ -888,7 +901,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, float p_margin, MotionResult *r_result) {
+bool PhysicsServerSW::body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, MotionResult *r_result) {
 
 	BodySW *body = body_owner.get(p_body);
 	ERR_FAIL_COND_V(!body, false);
@@ -897,7 +910,7 @@ 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_margin, r_result);
+	return body->get_space()->test_body_motion(body, p_from, p_motion, body->get_kinematic_margin(), r_result);
 }
 
 PhysicsDirectBodyState *PhysicsServerSW::body_get_direct_state(RID p_body) {
diff --git a/servers/physics/physics_server_sw.h b/servers/physics/physics_server_sw.h
index f9eb8fa454..fa754a1c8f 100644
--- a/servers/physics/physics_server_sw.h
+++ b/servers/physics/physics_server_sw.h
@@ -187,6 +187,9 @@ public:
 	virtual void body_set_param(RID p_body, BodyParameter p_param, real_t p_value);
 	virtual real_t body_get_param(RID p_body, BodyParameter p_param) const;
 
+	virtual void body_set_kinematic_safe_margin(RID p_body, real_t p_margin);
+	virtual real_t body_get_kinematic_safe_margin(RID p_body) const;
+
 	virtual void body_set_state(RID p_body, BodyState p_state, const Variant &p_variant);
 	virtual Variant body_get_state(RID p_body, BodyState p_state) const;
 
@@ -221,7 +224,7 @@ public:
 	virtual void body_set_ray_pickable(RID p_body, bool p_enable);
 	virtual bool body_is_ray_pickable(RID p_body) const;
 
-	virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, float p_margin = 0.001, MotionResult *r_result = NULL);
+	virtual bool body_test_motion(RID p_body, const Transform &p_from, const Vector3 &p_motion, MotionResult *r_result = NULL);
 
 	// this function only works on physics process, errors and returns null otherwise
 	virtual PhysicsDirectBodyState *body_get_direct_state(RID p_body);