-Re-added margins in one way collision (made in a more user friendly way than in Godot 2.1), fixes #23860

-Fixed potential bug in OWC (i dont think anyone had it but..)

16 files changed, 131 insertions, 11 deletions

diff --git a/scene/2d/collision_object_2d.cpp b/scene/2d/collision_object_2d.cpp
index 554b8dbc3d..d54070df8d 100644
--- a/scene/2d/collision_object_2d.cpp
+++ b/scene/2d/collision_object_2d.cpp
@@ -163,7 +163,7 @@ void CollisionObject2D::shape_owner_set_one_way_collision(uint32_t p_owner, bool
 	ShapeData &sd = shapes[p_owner];
 	sd.one_way_collision = p_enable;
 	for (int i = 0; i < sd.shapes.size(); i++) {
-		Physics2DServer::get_singleton()->body_set_shape_as_one_way_collision(rid, sd.shapes[i].index, p_enable);
+		Physics2DServer::get_singleton()->body_set_shape_as_one_way_collision(rid, sd.shapes[i].index, sd.one_way_collision, sd.one_way_collision_margin);
 	}
 }
 
@@ -174,6 +174,27 @@ bool CollisionObject2D::is_shape_owner_one_way_collision_enabled(uint32_t p_owne
 	return shapes[p_owner].one_way_collision;
 }
 
+void CollisionObject2D::shape_owner_set_one_way_collision_margin(uint32_t p_owner, float p_margin) {
+
+	if (area)
+		return; //not for areas
+
+	ERR_FAIL_COND(!shapes.has(p_owner));
+
+	ShapeData &sd = shapes[p_owner];
+	sd.one_way_collision_margin = p_margin;
+	for (int i = 0; i < sd.shapes.size(); i++) {
+		Physics2DServer::get_singleton()->body_set_shape_as_one_way_collision(rid, sd.shapes[i].index, sd.one_way_collision, sd.one_way_collision_margin);
+	}
+}
+
+float CollisionObject2D::get_shape_owner_one_way_collision_margin(uint32_t p_owner) const {
+
+	ERR_FAIL_COND_V(!shapes.has(p_owner), 0);
+
+	return shapes[p_owner].one_way_collision_margin;
+}
+
 void CollisionObject2D::get_shape_owners(List<uint32_t> *r_owners) {
 
 	for (Map<uint32_t, ShapeData>::Element *E = shapes.front(); E; E = E->next()) {
@@ -390,6 +411,8 @@ void CollisionObject2D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("is_shape_owner_disabled", "owner_id"), &CollisionObject2D::is_shape_owner_disabled);
 	ClassDB::bind_method(D_METHOD("shape_owner_set_one_way_collision", "owner_id", "enable"), &CollisionObject2D::shape_owner_set_one_way_collision);
 	ClassDB::bind_method(D_METHOD("is_shape_owner_one_way_collision_enabled", "owner_id"), &CollisionObject2D::is_shape_owner_one_way_collision_enabled);
+	ClassDB::bind_method(D_METHOD("shape_owner_set_one_way_collision_margin", "owner_id", "margin"), &CollisionObject2D::shape_owner_set_one_way_collision_margin);
+	ClassDB::bind_method(D_METHOD("get_shape_owner_one_way_collision_margin", "owner_id"), &CollisionObject2D::get_shape_owner_one_way_collision_margin);
 	ClassDB::bind_method(D_METHOD("shape_owner_add_shape", "owner_id", "shape"), &CollisionObject2D::shape_owner_add_shape);
 	ClassDB::bind_method(D_METHOD("shape_owner_get_shape_count", "owner_id"), &CollisionObject2D::shape_owner_get_shape_count);
 	ClassDB::bind_method(D_METHOD("shape_owner_get_shape", "owner_id", "shape_id"), &CollisionObject2D::shape_owner_get_shape);
diff --git a/scene/2d/collision_object_2d.h b/scene/2d/collision_object_2d.h
index d796c171bf..8aa3330f37 100644
--- a/scene/2d/collision_object_2d.h
+++ b/scene/2d/collision_object_2d.h
@@ -54,10 +54,12 @@ class CollisionObject2D : public Node2D {
 		Vector<Shape> shapes;
 		bool disabled;
 		bool one_way_collision;
+		float one_way_collision_margin;
 
 		ShapeData() {
 			disabled = false;
 			one_way_collision = false;
+			one_way_collision_margin = 0;
 			owner = NULL;
 		}
 	};
@@ -98,6 +100,9 @@ public:
 	void shape_owner_set_one_way_collision(uint32_t p_owner, bool p_enable);
 	bool is_shape_owner_one_way_collision_enabled(uint32_t p_owner) const;
 
+	void shape_owner_set_one_way_collision_margin(uint32_t p_owner, float p_margin);
+	float get_shape_owner_one_way_collision_margin(uint32_t p_owner) const;
+
 	void shape_owner_add_shape(uint32_t p_owner, const Ref<Shape2D> &p_shape);
 	int shape_owner_get_shape_count(uint32_t p_owner) const;
 	Ref<Shape2D> shape_owner_get_shape(uint32_t p_owner, int p_shape) const;
diff --git a/scene/2d/collision_polygon_2d.cpp b/scene/2d/collision_polygon_2d.cpp
index ac99a598d4..5edd49b3a3 100644
--- a/scene/2d/collision_polygon_2d.cpp
+++ b/scene/2d/collision_polygon_2d.cpp
@@ -122,6 +122,7 @@ void CollisionPolygon2D::_update_in_shape_owner(bool p_xform_only) {
 		return;
 	parent->shape_owner_set_disabled(owner_id, disabled);
 	parent->shape_owner_set_one_way_collision(owner_id, one_way_collision);
+	parent->shape_owner_set_one_way_collision_margin(owner_id, one_way_collision_margin);
 }
 
 void CollisionPolygon2D::_notification(int p_what) {
@@ -311,6 +312,16 @@ bool CollisionPolygon2D::is_one_way_collision_enabled() const {
 	return one_way_collision;
 }
 
+void CollisionPolygon2D::set_one_way_collision_margin(float p_margin) {
+	one_way_collision_margin = p_margin;
+	if (parent) {
+		parent->shape_owner_set_one_way_collision_margin(owner_id, one_way_collision_margin);
+	}
+}
+
+float CollisionPolygon2D::get_one_way_collision_margin() const {
+	return one_way_collision_margin;
+}
 void CollisionPolygon2D::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("set_polygon", "polygon"), &CollisionPolygon2D::set_polygon);
@@ -322,11 +333,14 @@ void CollisionPolygon2D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("is_disabled"), &CollisionPolygon2D::is_disabled);
 	ClassDB::bind_method(D_METHOD("set_one_way_collision", "enabled"), &CollisionPolygon2D::set_one_way_collision);
 	ClassDB::bind_method(D_METHOD("is_one_way_collision_enabled"), &CollisionPolygon2D::is_one_way_collision_enabled);
+	ClassDB::bind_method(D_METHOD("set_one_way_collision_margin", "margin"), &CollisionPolygon2D::set_one_way_collision_margin);
+	ClassDB::bind_method(D_METHOD("get_one_way_collision_margin"), &CollisionPolygon2D::get_one_way_collision_margin);
 
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "build_mode", PROPERTY_HINT_ENUM, "Solids,Segments"), "set_build_mode", "get_build_mode");
 	ADD_PROPERTY(PropertyInfo(Variant::POOL_VECTOR2_ARRAY, "polygon"), "set_polygon", "get_polygon");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "disabled"), "set_disabled", "is_disabled");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "one_way_collision"), "set_one_way_collision", "is_one_way_collision_enabled");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "one_way_collision_margin", PROPERTY_HINT_RANGE, "0,128,0.1"), "set_one_way_collision_margin", "get_one_way_collision_margin");
 
 	BIND_ENUM_CONSTANT(BUILD_SOLIDS);
 	BIND_ENUM_CONSTANT(BUILD_SEGMENTS);
@@ -341,4 +355,5 @@ CollisionPolygon2D::CollisionPolygon2D() {
 	owner_id = 0;
 	disabled = false;
 	one_way_collision = false;
+	one_way_collision_margin = 1.0;
 }
diff --git a/scene/2d/collision_polygon_2d.h b/scene/2d/collision_polygon_2d.h
index 1e9bcf4646..b88679f15b 100644
--- a/scene/2d/collision_polygon_2d.h
+++ b/scene/2d/collision_polygon_2d.h
@@ -54,6 +54,7 @@ protected:
 	CollisionObject2D *parent;
 	bool disabled;
 	bool one_way_collision;
+	float one_way_collision_margin;
 
 	Vector<Vector<Vector2> > _decompose_in_convex();
 
@@ -84,6 +85,9 @@ public:
 	void set_one_way_collision(bool p_enable);
 	bool is_one_way_collision_enabled() const;
 
+	void set_one_way_collision_margin(float p_margin);
+	float get_one_way_collision_margin() const;
+
 	CollisionPolygon2D();
 };
 
diff --git a/scene/2d/collision_shape_2d.cpp b/scene/2d/collision_shape_2d.cpp
index 1b648372c0..c756f49bbd 100644
--- a/scene/2d/collision_shape_2d.cpp
+++ b/scene/2d/collision_shape_2d.cpp
@@ -52,6 +52,7 @@ void CollisionShape2D::_update_in_shape_owner(bool p_xform_only) {
 		return;
 	parent->shape_owner_set_disabled(owner_id, disabled);
 	parent->shape_owner_set_one_way_collision(owner_id, one_way_collision);
+	parent->shape_owner_set_one_way_collision_margin(owner_id, one_way_collision_margin);
 }
 
 void CollisionShape2D::_notification(int p_what) {
@@ -219,6 +220,17 @@ bool CollisionShape2D::is_one_way_collision_enabled() const {
 	return one_way_collision;
 }
 
+void CollisionShape2D::set_one_way_collision_margin(float p_margin) {
+	one_way_collision_margin = p_margin;
+	if (parent) {
+		parent->shape_owner_set_one_way_collision_margin(owner_id, one_way_collision_margin);
+	}
+}
+
+float CollisionShape2D::get_one_way_collision_margin() const {
+	return one_way_collision_margin;
+}
+
 void CollisionShape2D::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("set_shape", "shape"), &CollisionShape2D::set_shape);
@@ -227,11 +239,14 @@ void CollisionShape2D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("is_disabled"), &CollisionShape2D::is_disabled);
 	ClassDB::bind_method(D_METHOD("set_one_way_collision", "enabled"), &CollisionShape2D::set_one_way_collision);
 	ClassDB::bind_method(D_METHOD("is_one_way_collision_enabled"), &CollisionShape2D::is_one_way_collision_enabled);
+	ClassDB::bind_method(D_METHOD("set_one_way_collision_margin", "margin"), &CollisionShape2D::set_one_way_collision_margin);
+	ClassDB::bind_method(D_METHOD("get_one_way_collision_margin"), &CollisionShape2D::get_one_way_collision_margin);
 	ClassDB::bind_method(D_METHOD("_shape_changed"), &CollisionShape2D::_shape_changed);
 
 	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "shape", PROPERTY_HINT_RESOURCE_TYPE, "Shape2D"), "set_shape", "get_shape");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "disabled"), "set_disabled", "is_disabled");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "one_way_collision"), "set_one_way_collision", "is_one_way_collision_enabled");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "one_way_collision_margin", PROPERTY_HINT_RANGE, "0,128,0.1"), "set_one_way_collision_margin", "get_one_way_collision_margin");
 }
 
 CollisionShape2D::CollisionShape2D() {
@@ -242,4 +257,5 @@ CollisionShape2D::CollisionShape2D() {
 	parent = NULL;
 	disabled = false;
 	one_way_collision = false;
+	one_way_collision_margin = 1.0;
 }
diff --git a/scene/2d/collision_shape_2d.h b/scene/2d/collision_shape_2d.h
index 8d74da0944..e913b4a866 100644
--- a/scene/2d/collision_shape_2d.h
+++ b/scene/2d/collision_shape_2d.h
@@ -46,6 +46,7 @@ class CollisionShape2D : public Node2D {
 	void _shape_changed();
 	bool disabled;
 	bool one_way_collision;
+	float one_way_collision_margin;
 
 	void _update_in_shape_owner(bool p_xform_only = false);
 
@@ -65,6 +66,9 @@ public:
 	void set_one_way_collision(bool p_enable);
 	bool is_one_way_collision_enabled() const;
 
+	void set_one_way_collision_margin(float p_margin);
+	float get_one_way_collision_margin() const;
+
 	virtual String get_configuration_warning() const;
 
 	CollisionShape2D();
diff --git a/scene/2d/tile_map.cpp b/scene/2d/tile_map.cpp
index d61296b13f..44730062c4 100644
--- a/scene/2d/tile_map.cpp
+++ b/scene/2d/tile_map.cpp
@@ -481,7 +481,7 @@ void TileMap::update_dirty_quadrants() {
 						}
 						ps->body_add_shape(q.body, shape->get_rid(), xform);
 						ps->body_set_shape_metadata(q.body, shape_idx, Vector2(E->key().x, E->key().y));
-						ps->body_set_shape_as_one_way_collision(q.body, shape_idx, shapes[i].one_way_collision);
+						ps->body_set_shape_as_one_way_collision(q.body, shape_idx, shapes[i].one_way_collision, shapes[i].one_way_collision_margin);
 						shape_idx++;
 					}
 				}
diff --git a/scene/resources/tile_set.cpp b/scene/resources/tile_set.cpp
index 9cb4972a73..38a5f9714e 100644
--- a/scene/resources/tile_set.cpp
+++ b/scene/resources/tile_set.cpp
@@ -154,6 +154,8 @@ bool TileSet::_set(const StringName &p_name, const Variant &p_value) {
 		tile_set_shape_transform(id, 0, p_value);
 	else if (what == "shape_one_way")
 		tile_set_shape_one_way(id, 0, p_value);
+	else if (what == "shape_one_way_margin")
+		tile_set_shape_one_way_margin(id, 0, p_value);
 	else if (what == "shapes")
 		_tile_set_shapes(id, p_value);
 	else if (what == "occluder")
@@ -266,6 +268,8 @@ bool TileSet::_get(const StringName &p_name, Variant &r_ret) const {
 		r_ret = tile_get_shape_transform(id, 0);
 	else if (what == "shape_one_way")
 		r_ret = tile_get_shape_one_way(id, 0);
+	else if (what == "shape_one_way_margin")
+		r_ret = tile_get_shape_one_way_margin(id, 0);
 	else if (what == "shapes")
 		r_ret = _tile_get_shapes(id);
 	else if (what == "occluder")
@@ -324,6 +328,7 @@ void TileSet::_get_property_list(List<PropertyInfo> *p_list) const {
 		p_list->push_back(PropertyInfo(Variant::VECTOR2, pre + "shape_transform", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_EDITOR));
 		p_list->push_back(PropertyInfo(Variant::OBJECT, pre + "shape", PROPERTY_HINT_RESOURCE_TYPE, "Shape2D", PROPERTY_USAGE_EDITOR));
 		p_list->push_back(PropertyInfo(Variant::BOOL, pre + "shape_one_way", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_EDITOR));
+		p_list->push_back(PropertyInfo(Variant::REAL, pre + "shape_one_way_margin", PROPERTY_HINT_RANGE, "0,128,0.01", PROPERTY_USAGE_EDITOR));
 		p_list->push_back(PropertyInfo(Variant::ARRAY, pre + "shapes", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NOEDITOR));
 		p_list->push_back(PropertyInfo(Variant::INT, pre + "z_index", PROPERTY_HINT_RANGE, itos(VS::CANVAS_ITEM_Z_MIN) + "," + itos(VS::CANVAS_ITEM_Z_MAX) + ",1"));
 	}
@@ -715,6 +720,22 @@ bool TileSet::tile_get_shape_one_way(int p_id, int p_shape_id) const {
 	return false;
 }
 
+void TileSet::tile_set_shape_one_way_margin(int p_id, int p_shape_id, float p_margin) {
+	ERR_FAIL_COND(!tile_map.has(p_id));
+	if (tile_map[p_id].shapes_data.size() <= p_shape_id)
+		tile_map[p_id].shapes_data.resize(p_shape_id + 1);
+	tile_map[p_id].shapes_data.write[p_shape_id].one_way_collision_margin = p_margin;
+	emit_changed();
+}
+
+float TileSet::tile_get_shape_one_way_margin(int p_id, int p_shape_id) const {
+	ERR_FAIL_COND_V(!tile_map.has(p_id), 0);
+	if (tile_map[p_id].shapes_data.size() > p_shape_id)
+		return tile_map[p_id].shapes_data[p_shape_id].one_way_collision_margin;
+
+	return 0;
+}
+
 void TileSet::tile_set_light_occluder(int p_id, const Ref<OccluderPolygon2D> &p_light_occluder) {
 
 	ERR_FAIL_COND(!tile_map.has(p_id));
@@ -884,6 +905,11 @@ void TileSet::_tile_set_shapes(int p_id, const Array &p_shapes) {
 			else
 				s.one_way_collision = default_one_way;
 
+			if (d.has("one_way_margin") && d["one_way_margin"].is_num())
+				s.one_way_collision_margin = d["one_way_margin"];
+			else
+				s.one_way_collision = 1.0;
+
 			if (d.has("autotile_coord") && d["autotile_coord"].get_type() == Variant::VECTOR2)
 				s.autotile_coord = d["autotile_coord"];
 			else
@@ -911,6 +937,7 @@ Array TileSet::_tile_get_shapes(int p_id) const {
 		shape_data["shape"] = data[i].shape;
 		shape_data["shape_transform"] = data[i].shape_transform;
 		shape_data["one_way"] = data[i].one_way_collision;
+		shape_data["one_way_margin"] = data[i].one_way_collision_margin;
 		shape_data["autotile_coord"] = data[i].autotile_coord;
 		arr.push_back(shape_data);
 	}
@@ -1034,6 +1061,8 @@ void TileSet::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("tile_get_shape_transform", "id", "shape_id"), &TileSet::tile_get_shape_transform);
 	ClassDB::bind_method(D_METHOD("tile_set_shape_one_way", "id", "shape_id", "one_way"), &TileSet::tile_set_shape_one_way);
 	ClassDB::bind_method(D_METHOD("tile_get_shape_one_way", "id", "shape_id"), &TileSet::tile_get_shape_one_way);
+	ClassDB::bind_method(D_METHOD("tile_set_shape_one_way_margin", "id", "shape_id", "one_way"), &TileSet::tile_set_shape_one_way_margin);
+	ClassDB::bind_method(D_METHOD("tile_get_shape_one_way_margin", "id", "shape_id"), &TileSet::tile_get_shape_one_way_margin);
 	ClassDB::bind_method(D_METHOD("tile_add_shape", "id", "shape", "shape_transform", "one_way", "autotile_coord"), &TileSet::tile_add_shape, DEFVAL(false), DEFVAL(Vector2()));
 	ClassDB::bind_method(D_METHOD("tile_get_shape_count", "id"), &TileSet::tile_get_shape_count);
 	ClassDB::bind_method(D_METHOD("tile_set_shapes", "id", "shapes"), &TileSet::_tile_set_shapes);
diff --git a/scene/resources/tile_set.h b/scene/resources/tile_set.h
index df162c9210..4800371d3c 100644
--- a/scene/resources/tile_set.h
+++ b/scene/resources/tile_set.h
@@ -48,9 +48,11 @@ public:
 		Transform2D shape_transform;
 		Vector2 autotile_coord;
 		bool one_way_collision;
+		float one_way_collision_margin;
 
 		ShapeData() {
 			one_way_collision = false;
+			one_way_collision_margin = 1.0;
 		}
 	};
 
@@ -194,6 +196,9 @@ public:
 	void tile_set_shape_one_way(int p_id, int p_shape_id, bool p_one_way);
 	bool tile_get_shape_one_way(int p_id, int p_shape_id) const;
 
+	void tile_set_shape_one_way_margin(int p_id, int p_shape_id, float p_margin);
+	float tile_get_shape_one_way_margin(int p_id, int p_shape_id) const;
+
 	void tile_clear_shapes(int p_id);
 	void tile_add_shape(int p_id, const Ref<Shape2D> &p_shape, const Transform2D &p_transform, bool p_one_way = false, const Vector2 &p_autotile_coord = Vector2());
 	int tile_get_shape_count(int p_id) const;
diff --git a/servers/physics_2d/collision_object_2d_sw.cpp b/servers/physics_2d/collision_object_2d_sw.cpp
index 4fbc0769ba..4ec92497e7 100644
--- a/servers/physics_2d/collision_object_2d_sw.cpp
+++ b/servers/physics_2d/collision_object_2d_sw.cpp
@@ -40,6 +40,7 @@ void CollisionObject2DSW::add_shape(Shape2DSW *p_shape, const Transform2D &p_tra
 	s.bpid = 0; //needs update
 	s.disabled = false;
 	s.one_way_collision = false;
+	s.one_way_collision_margin = 0;
 	shapes.push_back(s);
 	p_shape->add_owner(this);
 	_update_shapes();
diff --git a/servers/physics_2d/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h
index 6946217dbd..d5e815ff45 100644
--- a/servers/physics_2d/collision_object_2d_sw.h
+++ b/servers/physics_2d/collision_object_2d_sw.h
@@ -62,9 +62,11 @@ private:
 		Variant metadata;
 		bool disabled;
 		bool one_way_collision;
+		float one_way_collision_margin;
 		Shape() {
 			disabled = false;
 			one_way_collision = false;
+			one_way_collision_margin = 0;
 		}
 	};
 
@@ -150,15 +152,21 @@ public:
 		return shapes[p_idx].disabled;
 	}
 
-	_FORCE_INLINE_ void set_shape_as_one_way_collision(int p_idx, bool p_one_way_collision) {
+	_FORCE_INLINE_ void set_shape_as_one_way_collision(int p_idx, bool p_one_way_collision, float p_margin) {
 		CRASH_BAD_INDEX(p_idx, shapes.size());
 		shapes.write[p_idx].one_way_collision = p_one_way_collision;
+		shapes.write[p_idx].one_way_collision_margin = p_margin;
 	}
 	_FORCE_INLINE_ bool is_shape_set_as_one_way_collision(int p_idx) const {
 		CRASH_BAD_INDEX(p_idx, shapes.size());
 		return shapes[p_idx].one_way_collision;
 	}
 
+	_FORCE_INLINE_ float get_shape_one_way_collision_margin(int p_idx) const {
+		CRASH_BAD_INDEX(p_idx, shapes.size());
+		return shapes[p_idx].one_way_collision_margin;
+	}
+
 	void set_collision_mask(uint32_t p_mask) { collision_mask = p_mask; }
 	_FORCE_INLINE_ uint32_t get_collision_mask() const { return collision_mask; }
 
diff --git a/servers/physics_2d/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp
index daea013d35..4d1c56b843 100644
--- a/servers/physics_2d/physics_2d_server_sw.cpp
+++ b/servers/physics_2d/physics_2d_server_sw.cpp
@@ -209,12 +209,14 @@ void Physics2DServerSW::_shape_col_cbk(const Vector2 &p_point_A, const Vector2 &
 			return;
 		cbk->ptr[min_depth_idx * 2 + 0] = p_point_A;
 		cbk->ptr[min_depth_idx * 2 + 1] = p_point_B;
+		cbk->passed++;
 
 	} else {
 
 		cbk->ptr[cbk->amount * 2 + 0] = p_point_A;
 		cbk->ptr[cbk->amount * 2 + 1] = p_point_B;
 		cbk->amount++;
+		cbk->passed++;
 	}
 }
 
@@ -233,6 +235,7 @@ bool Physics2DServerSW::shape_collide(RID p_shape_A, const Transform2D &p_xform_
 	CollCbkData cbk;
 	cbk.max = p_result_max;
 	cbk.amount = 0;
+	cbk.passed = 0;
 	cbk.ptr = r_results;
 
 	bool res = CollisionSolver2DSW::solve(shape_A, p_xform_A, p_motion_A, shape_B, p_xform_B, p_motion_B, _shape_col_cbk, &cbk);
@@ -740,14 +743,14 @@ void Physics2DServerSW::body_set_shape_disabled(RID p_body, int p_shape_idx, boo
 
 	body->set_shape_as_disabled(p_shape_idx, p_disabled);
 }
-void Physics2DServerSW::body_set_shape_as_one_way_collision(RID p_body, int p_shape_idx, bool p_enable) {
+void Physics2DServerSW::body_set_shape_as_one_way_collision(RID p_body, int p_shape_idx, bool p_enable, float p_margin) {
 
 	Body2DSW *body = body_owner.get(p_body);
 	ERR_FAIL_COND(!body);
 
 	ERR_FAIL_INDEX(p_shape_idx, body->get_shape_count());
 
-	body->set_shape_as_one_way_collision(p_shape_idx, p_enable);
+	body->set_shape_as_one_way_collision(p_shape_idx, p_enable, p_margin);
 }
 
 void Physics2DServerSW::body_set_continuous_collision_detection_mode(RID p_body, CCDMode p_mode) {
diff --git a/servers/physics_2d/physics_2d_server_sw.h b/servers/physics_2d/physics_2d_server_sw.h
index da402ee16e..4beec41442 100644
--- a/servers/physics_2d/physics_2d_server_sw.h
+++ b/servers/physics_2d/physics_2d_server_sw.h
@@ -80,6 +80,7 @@ public:
 		real_t valid_depth;
 		int max;
 		int amount;
+		int passed;
 		int invalid_by_dir;
 		Vector2 *ptr;
 	};
@@ -188,7 +189,7 @@ public:
 	virtual void body_clear_shapes(RID p_body);
 
 	virtual void body_set_shape_disabled(RID p_body, int p_shape_idx, bool p_disabled);
-	virtual void body_set_shape_as_one_way_collision(RID p_body, int p_shape_idx, bool p_enable);
+	virtual void body_set_shape_as_one_way_collision(RID p_body, int p_shape_idx, bool p_enable, float p_margin);
 
 	virtual void body_attach_object_instance_id(RID p_body, uint32_t p_ID);
 	virtual uint32_t body_get_object_instance_id(RID p_body) const;
diff --git a/servers/physics_2d/physics_2d_server_wrap_mt.h b/servers/physics_2d/physics_2d_server_wrap_mt.h
index 467d301ad5..5b9a102a1b 100644
--- a/servers/physics_2d/physics_2d_server_wrap_mt.h
+++ b/servers/physics_2d/physics_2d_server_wrap_mt.h
@@ -194,7 +194,7 @@ public:
 	FUNC2RC(RID, body_get_shape, RID, int);
 
 	FUNC3(body_set_shape_disabled, RID, int, bool);
-	FUNC3(body_set_shape_as_one_way_collision, RID, int, bool);
+	FUNC4(body_set_shape_as_one_way_collision, RID, int, bool, float);
 
 	FUNC2(body_remove_shape, RID, int);
 	FUNC1(body_clear_shapes, RID);
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index 3d8005cd4d..77af0957e1 100644
--- a/servers/physics_2d/space_2d_sw.cpp
+++ b/servers/physics_2d/space_2d_sw.cpp
@@ -328,6 +328,7 @@ bool Physics2DDirectSpaceStateSW::collide_shape(RID p_shape, const Transform2D &
 	Physics2DServerSW::CollCbkData cbk;
 	cbk.max = p_result_max;
 	cbk.amount = 0;
+	cbk.passed = 0;
 	cbk.ptr = r_results;
 	CollisionSolver2DSW::CallbackResult cbkres = NULL;
 
@@ -573,6 +574,7 @@ int Space2DSW::test_body_ray_separation(Body2DSW *p_body, const Transform2D &p_t
 					int shape_idx = intersection_query_subindex_results[i];
 
 					cbk.amount = 0;
+					cbk.passed = 0;
 					cbk.ptr = sr;
 					cbk.invalid_by_dir = 0;
 
@@ -720,6 +722,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 			Physics2DServerSW::CollCbkData cbk;
 			cbk.max = max_results;
 			cbk.amount = 0;
+			cbk.passed = 0;
 			cbk.ptr = sr;
 			cbk.invalid_by_dir = 0;
 			excluded_shape_pair_count = 0; //last step is the one valid
@@ -759,7 +762,9 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 
 						cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized();
 
-						cbk.valid_depth = p_margin; //only valid depth is the collision margin
+						float owc_margin = col_obj->get_shape_one_way_collision_margin(shape_idx);
+						print_line("margin: " + rtos(owc_margin));
+						cbk.valid_depth = MAX(owc_margin, p_margin); //user specified, but never less than actual margin or it wont work
 						cbk.invalid_by_dir = 0;
 
 						if (col_obj->get_type() == CollisionObject2DSW::TYPE_BODY) {
@@ -780,12 +785,12 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 						cbk.invalid_by_dir = 0;
 					}
 
-					int current_collisions = cbk.amount;
+					int current_passed = cbk.passed; //save how many points passed collision
 					bool did_collide = false;
 
 					Shape2DSW *against_shape = col_obj->get_shape(shape_idx);
 					if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), against_shape, col_obj_shape_xform, Vector2(), cbkres, cbkptr, NULL, separation_margin)) {
-						did_collide = cbk.amount > current_collisions;
+						did_collide = cbk.passed > current_passed; //more passed, so collision actually existed
 					}
 
 					if (!did_collide && cbk.invalid_by_dir > 0) {
@@ -933,6 +938,7 @@ bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, co
 					Physics2DServerSW::CollCbkData cbk;
 					cbk.max = 1;
 					cbk.amount = 0;
+					cbk.passed = 0;
 					cbk.ptr = cd;
 					cbk.valid_dir = col_obj_shape_xform.get_axis(1).normalized();
 
diff --git a/servers/physics_2d_server.h b/servers/physics_2d_server.h
index 86375b8e9e..df7fe46f76 100644
--- a/servers/physics_2d_server.h
+++ b/servers/physics_2d_server.h
@@ -398,7 +398,7 @@ public:
 	virtual Variant body_get_shape_metadata(RID p_body, int p_shape_idx) const = 0;
 
 	virtual void body_set_shape_disabled(RID p_body, int p_shape, bool p_disabled) = 0;
-	virtual void body_set_shape_as_one_way_collision(RID p_body, int p_shape, bool p_enabled) = 0;
+	virtual void body_set_shape_as_one_way_collision(RID p_body, int p_shape, bool p_enabled, float p_margin = 0) = 0;
 
 	virtual void body_remove_shape(RID p_body, int p_shape_idx) = 0;
 	virtual void body_clear_shapes(RID p_body) = 0;