3 files changed, 61 insertions, 1 deletions

diff --git a/servers/physics_2d/body_2d_sw.cpp b/servers/physics_2d/body_2d_sw.cpp
index 464b818384..0ba661b4c4 100644
--- a/servers/physics_2d/body_2d_sw.cpp
+++ b/servers/physics_2d/body_2d_sw.cpp
@@ -657,6 +657,7 @@ Body2DSW::Body2DSW() : CollisionObject2DSW(TYPE_BODY), active_list(this), inerti
 	area_linear_damp=0;
 	contact_count=0;
 	gravity_scale=1.0;
+	using_one_way_cache=false;
 	one_way_collision_max_depth=0.1;
 
 	still_time=0;
diff --git a/servers/physics_2d/body_2d_sw.h b/servers/physics_2d/body_2d_sw.h
index ca4d80a15b..e34686f3ac 100644
--- a/servers/physics_2d/body_2d_sw.h
+++ b/servers/physics_2d/body_2d_sw.h
@@ -81,6 +81,7 @@ class Body2DSW : public CollisionObject2DSW {
 	bool active;
 	bool can_sleep;
 	bool first_time_kinematic;
+	bool using_one_way_cache;
 	void _update_inertia();
 	virtual void _shapes_changed();
 	Matrix32 new_transform;
@@ -229,12 +230,17 @@ public:
 	_FORCE_INLINE_ void set_continuous_collision_detection_mode(Physics2DServer::CCDMode p_mode) { continuous_cd_mode=p_mode; }
 	_FORCE_INLINE_ Physics2DServer::CCDMode get_continuous_collision_detection_mode() const { return continuous_cd_mode; }
 
-	void set_one_way_collision_direction(const Vector2& p_dir) { one_way_collision_direction=p_dir; }
+	void set_one_way_collision_direction(const Vector2& p_dir) {
+		one_way_collision_direction=p_dir;
+		using_one_way_cache=one_way_collision_direction!=Vector2();
+	}
 	Vector2 get_one_way_collision_direction() const { return one_way_collision_direction; }
 
 	void set_one_way_collision_max_depth(float p_depth) { one_way_collision_max_depth=p_depth; }
 	float get_one_way_collision_max_depth() const { return one_way_collision_max_depth; }
 
+	_FORCE_INLINE_ bool is_using_one_way_collision() const { return using_one_way_cache; }
+
 	void set_space(Space2DSW *p_space);
 
 	void update_inertias();
diff --git a/servers/physics_2d/body_pair_2d_sw.cpp b/servers/physics_2d/body_pair_2d_sw.cpp
index 8913e396d2..e8d37d346a 100644
--- a/servers/physics_2d/body_pair_2d_sw.cpp
+++ b/servers/physics_2d/body_pair_2d_sw.cpp
@@ -265,6 +265,8 @@ bool BodyPair2DSW::setup(float p_step) {
 	} 
 	//faster to set than to check..
 
+	bool prev_collided=collided;
+
 	collided = CollisionSolver2DSW::solve(shape_A_ptr,xform_A,motion_A,shape_B_ptr,xform_B,motion_B,_add_contact,this,&sep_axis);
 	if (!collided) {
 
@@ -285,6 +287,57 @@ bool BodyPair2DSW::setup(float p_step) {
 
 	}
 
+	if (!prev_collided) {
+
+		if (A->is_using_one_way_collision()) {
+			Vector2 direction = A->get_one_way_collision_direction();
+			bool valid=false;
+			for(int i=0;i<contact_count;i++) {
+				Contact& c = contacts[i];
+
+				if (c.normal.dot(direction)<0)
+					continue;
+				if (B->get_linear_velocity().dot(direction)<0)
+					continue;
+
+				if (!c.reused) {
+					continue;
+				}
+
+				valid=true;
+			}
+
+			if (!valid) {
+				collided=false;
+				return false;
+			}
+		}
+
+		if (B->is_using_one_way_collision()) {
+			Vector2 direction = B->get_one_way_collision_direction();
+			bool valid=false;
+			for(int i=0;i<contact_count;i++) {
+
+				Contact& c = contacts[i];
+
+				if (c.normal.dot(direction)<0)
+					continue;
+				if (A->get_linear_velocity().dot(direction)<0)
+					continue;
+
+				if (!c.reused) {
+					continue;
+				}
+
+				valid=true;
+			}
+			if (!valid) {
+				collided=false;
+				return false;
+			}
+		}
+	}
+
 	real_t max_penetration = space->get_contact_max_allowed_penetration();
 
 	float bias = 0.3f;