12 files changed, 371 insertions, 5 deletions

diff --git a/servers/physics_2d/area_2d_sw.cpp b/servers/physics_2d/area_2d_sw.cpp
index 2e911288ae..65f3b80dd3 100644
--- a/servers/physics_2d/area_2d_sw.cpp
+++ b/servers/physics_2d/area_2d_sw.cpp
@@ -31,6 +31,7 @@
 #include "body_2d_sw.h"
 
 Area2DSW::BodyKey::BodyKey(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) { rid=p_body->get_self(); instance_id=p_body->get_instance_id(); body_shape=p_body_shape; area_shape=p_area_shape; }
+Area2DSW::BodyKey::BodyKey(Area2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) { rid=p_body->get_self(); instance_id=p_body->get_instance_id(); body_shape=p_body_shape; area_shape=p_area_shape; }
 
 void Area2DSW::_shapes_changed() {
 
@@ -57,6 +58,7 @@ void Area2DSW::set_space(Space2DSW *p_space) {
 	}
 
 	monitored_bodies.clear();
+	monitored_areas.clear();
 
 	_set_space(p_space);
 }
@@ -76,11 +78,31 @@ void Area2DSW::set_monitor_callback(ObjectID p_id, const StringName& p_method) {
 	monitor_callback_method=p_method;
 
 	monitored_bodies.clear();
+	monitored_areas.clear();
 
 	_shape_changed();
 
 }
 
+void Area2DSW::set_area_monitor_callback(ObjectID p_id, const StringName& p_method) {
+
+
+	if (p_id==area_monitor_callback_id) {
+		area_monitor_callback_method=p_method;
+		return;
+	}
+
+	_unregister_shapes();
+
+	area_monitor_callback_id=p_id;
+	area_monitor_callback_method=p_method;
+
+	monitored_bodies.clear();
+	monitored_areas.clear();
+
+	_shape_changed();
+
+}
 
 
 void Area2DSW::set_space_override_mode(Physics2DServer::AreaSpaceOverrideMode p_mode) {
@@ -134,6 +156,15 @@ void Area2DSW::_queue_monitor_update() {
 
 }
 
+void Area2DSW::set_monitorable(bool p_monitorable) {
+
+	if (monitorable==p_monitorable)
+		return;
+
+	monitorable=p_monitorable;
+	_set_static(!monitorable);
+}
+
 void Area2DSW::call_queries() {
 
 	if (monitor_callback_id && !monitored_bodies.empty()) {
@@ -170,13 +201,49 @@ void Area2DSW::call_queries() {
 
 	monitored_bodies.clear();
 
+	if (area_monitor_callback_id && !monitored_areas.empty()) {
+
+
+		Variant res[5];
+		Variant *resptr[5];
+		for(int i=0;i<5;i++)
+			resptr[i]=&res[i];
+
+		Object *obj = ObjectDB::get_instance(area_monitor_callback_id);
+		if (!obj) {
+			monitored_areas.clear();
+			area_monitor_callback_id=0;
+			return;
+		}
+
+
+
+		for (Map<BodyKey,BodyState>::Element *E=monitored_areas.front();E;E=E->next()) {
+
+			if (E->get().state==0)
+				continue; //nothing happened
+
+			res[0]=E->get().state>0 ? Physics2DServer::AREA_BODY_ADDED : Physics2DServer::AREA_BODY_REMOVED;
+			res[1]=E->key().rid;
+			res[2]=E->key().instance_id;
+			res[3]=E->key().body_shape;
+			res[4]=E->key().area_shape;
+
+
+			Variant::CallError ce;
+			obj->call(area_monitor_callback_method,(const Variant**)resptr,5,ce);
+		}
+	}
+
+	monitored_areas.clear();
+
 	//get_space()->area_remove_from_monitor_query_list(&monitor_query_list);
 
 }
 
 Area2DSW::Area2DSW() : CollisionObject2DSW(TYPE_AREA), monitor_query_list(this), moved_list(this)  {
 
-	_set_static(true); //areas are never active
+	_set_static(true); //areas are not active by default
 	space_override_mode=Physics2DServer::AREA_SPACE_OVERRIDE_DISABLED;
 	gravity=9.80665;
 	gravity_vector=Vector2(0,-1);
@@ -187,6 +254,8 @@ Area2DSW::Area2DSW() : CollisionObject2DSW(TYPE_AREA), monitor_query_list(this),
 	linear_damp=0.1;
 	priority=0;
 	monitor_callback_id=0;
+	area_monitor_callback_id=0;
+	monitorable=false;
 
 
 }
diff --git a/servers/physics_2d/area_2d_sw.h b/servers/physics_2d/area_2d_sw.h
index d94b2f9ccf..26b7b2516c 100644
--- a/servers/physics_2d/area_2d_sw.h
+++ b/servers/physics_2d/area_2d_sw.h
@@ -50,10 +50,14 @@ class Area2DSW : public CollisionObject2DSW{
 	float linear_damp;
 	float angular_damp;
 	int priority;
+	bool monitorable;
 
 	ObjectID monitor_callback_id;
 	StringName monitor_callback_method;
 
+	ObjectID area_monitor_callback_id;
+	StringName area_monitor_callback_method;
+
 	SelfList<Area2DSW> monitor_query_list;
 	SelfList<Area2DSW> moved_list;
 
@@ -80,6 +84,7 @@ class Area2DSW : public CollisionObject2DSW{
 
 		_FORCE_INLINE_ BodyKey() {}
 		BodyKey(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape);
+		BodyKey(Area2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape);
 	};
 
 	struct BodyState {
@@ -91,6 +96,7 @@ class Area2DSW : public CollisionObject2DSW{
 	};
 
 	Map<BodyKey,BodyState> monitored_bodies;
+	Map<BodyKey,BodyState> monitored_areas;
 
 	//virtual void shape_changed_notify(Shape2DSW *p_shape);
 	//virtual void shape_deleted_notify(Shape2DSW *p_shape);
@@ -108,9 +114,16 @@ public:
 	void set_monitor_callback(ObjectID p_id, const StringName& p_method);
 	_FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback_id; }
 
+	void set_area_monitor_callback(ObjectID p_id, const StringName& p_method);
+	_FORCE_INLINE_ bool has_area_monitor_callback() const { return area_monitor_callback_id; }
+
+
 	_FORCE_INLINE_ void add_body_to_query(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape);
 	_FORCE_INLINE_ void remove_body_from_query(Body2DSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape);
 
+	_FORCE_INLINE_ void add_area_to_query(Area2DSW *p_area, uint32_t p_area_shape,uint32_t p_self_shape);
+	_FORCE_INLINE_ void remove_area_from_query(Area2DSW *p_area, uint32_t p_area_shape,uint32_t p_self_shape);
+
 	void set_param(Physics2DServer::AreaParameter p_param, const Variant& p_value);
 	Variant get_param(Physics2DServer::AreaParameter p_param) const;
 
@@ -142,6 +155,9 @@ public:
 	_FORCE_INLINE_ void remove_constraint( Constraint2DSW* p_constraint) { constraints.erase(p_constraint); }
 	_FORCE_INLINE_ const Set<Constraint2DSW*>& get_constraints() const { return constraints; }
 
+	void set_monitorable(bool p_monitorable);
+	_FORCE_INLINE_ bool is_monitorable() const { return monitorable; }
+
 	void set_transform(const Matrix32& p_transform);
 
 	void set_space(Space2DSW *p_space);
@@ -168,6 +184,24 @@ void Area2DSW::remove_body_from_query(Body2DSW *p_body, uint32_t p_body_shape,ui
 		_queue_monitor_update();
 }
 
+void Area2DSW::add_area_to_query(Area2DSW *p_area, uint32_t p_area_shape,uint32_t p_self_shape) {
+
+
+	BodyKey bk(p_area,p_area_shape,p_self_shape);
+	monitored_areas[bk].inc();
+	if (!monitor_query_list.in_list())
+		_queue_monitor_update();
+
+
+}
+void Area2DSW::remove_area_from_query(Area2DSW *p_area, uint32_t p_area_shape,uint32_t p_self_shape) {
+
+
+	BodyKey bk(p_area,p_area_shape,p_self_shape);
+	monitored_areas[bk].dec();
+	if (!monitor_query_list.in_list())
+		_queue_monitor_update();
+}
 
 
 
diff --git a/servers/physics_2d/area_pair_2d_sw.cpp b/servers/physics_2d/area_pair_2d_sw.cpp
index 1c7f73db5e..ed2e34c972 100644
--- a/servers/physics_2d/area_pair_2d_sw.cpp
+++ b/servers/physics_2d/area_pair_2d_sw.cpp
@@ -94,3 +94,72 @@ AreaPair2DSW::~AreaPair2DSW() {
 	body->remove_constraint(this);
 	area->remove_constraint(this);
 }
+
+
+//////////////////////////////////
+
+
+
+bool Area2Pair2DSW::setup(float p_step) {
+
+	bool result = CollisionSolver2DSW::solve(area_a->get_shape(shape_a),area_a->get_transform() * area_a->get_shape_transform(shape_a),Vector2(),area_b->get_shape(shape_b),area_b->get_transform() * area_b->get_shape_transform(shape_b),Vector2(),NULL,this);
+
+	if (result!=colliding) {
+
+		if (result) {
+
+			if (area_b->has_area_monitor_callback() && area_a->is_monitorable())
+				area_b->add_area_to_query(area_a,shape_a,shape_b);
+
+			if (area_a->has_area_monitor_callback() && area_b->is_monitorable())
+				area_a->add_area_to_query(area_b,shape_b,shape_a);
+
+		} else {
+
+			if (area_b->has_area_monitor_callback() && area_a->is_monitorable())
+				area_b->remove_area_from_query(area_a,shape_a,shape_b);
+
+			if (area_a->has_area_monitor_callback() && area_b->is_monitorable())
+				area_a->remove_area_from_query(area_b,shape_b,shape_a);
+		}
+
+		colliding=result;
+
+	}
+
+	return false; //never do any post solving
+}
+
+void Area2Pair2DSW::solve(float p_step) {
+
+
+}
+
+
+Area2Pair2DSW::Area2Pair2DSW(Area2DSW *p_area_a,int p_shape_a, Area2DSW *p_area_b,int p_shape_b) {
+
+
+	area_a=p_area_a;
+	area_b=p_area_b;
+	shape_a=p_shape_a;
+	shape_b=p_shape_b;
+	colliding=false;
+	area_a->add_constraint(this);
+	area_b->add_constraint(this);
+
+}
+
+Area2Pair2DSW::~Area2Pair2DSW() {
+
+	if (colliding) {
+
+		if (area_b->has_area_monitor_callback() && area_a->is_monitorable())
+			area_b->remove_area_from_query(area_a,shape_a,shape_b);
+
+		if (area_a->has_area_monitor_callback() && area_b->is_monitorable())
+			area_a->remove_area_from_query(area_b,shape_b,shape_a);
+	}
+
+	area_a->remove_constraint(this);
+	area_b->remove_constraint(this);
+}
diff --git a/servers/physics_2d/area_pair_2d_sw.h b/servers/physics_2d/area_pair_2d_sw.h
index 5dad8c7a12..575490b109 100644
--- a/servers/physics_2d/area_pair_2d_sw.h
+++ b/servers/physics_2d/area_pair_2d_sw.h
@@ -49,5 +49,23 @@ public:
 	~AreaPair2DSW();
 };
 
+
+class Area2Pair2DSW : public Constraint2DSW {
+
+	Area2DSW *area_a;
+	Area2DSW *area_b;
+	int shape_a;
+	int shape_b;
+	bool colliding;
+public:
+
+	bool setup(float p_step);
+	void solve(float p_step);
+
+	Area2Pair2DSW(Area2DSW *p_area_a,int p_shape_a, Area2DSW *p_area_b,int p_shape_b);
+	~Area2Pair2DSW();
+};
+
+
 #endif // AREA_PAIR_2D_SW_H
 
diff --git a/servers/physics_2d/collision_object_2d_sw.cpp b/servers/physics_2d/collision_object_2d_sw.cpp
index d0443f8110..eefc598b39 100644
--- a/servers/physics_2d/collision_object_2d_sw.cpp
+++ b/servers/physics_2d/collision_object_2d_sw.cpp
@@ -228,4 +228,5 @@ CollisionObject2DSW::CollisionObject2DSW(Type p_type) {
 	instance_id=0;
 	user_mask=0;
 	layer_mask=1;
+	pickable=true;
 }
diff --git a/servers/physics_2d/collision_object_2d_sw.h b/servers/physics_2d/collision_object_2d_sw.h
index 00ad361245..0c91237876 100644
--- a/servers/physics_2d/collision_object_2d_sw.h
+++ b/servers/physics_2d/collision_object_2d_sw.h
@@ -47,6 +47,7 @@ private:
 	Type type;
 	RID self;
 	ObjectID instance_id;
+	bool pickable;
 
 	struct Shape {
 
@@ -129,6 +130,9 @@ public:
 
 	_FORCE_INLINE_ bool is_static() const { return _static;  }
 
+	void set_pickable(bool p_pickable) { pickable=p_pickable; }
+	_FORCE_INLINE_ bool is_pickable() const { return pickable; }
+
 	virtual ~CollisionObject2DSW() {}
 
 };
diff --git a/servers/physics_2d/physics_2d_server_sw.cpp b/servers/physics_2d/physics_2d_server_sw.cpp
index be49955055..883acd0200 100644
--- a/servers/physics_2d/physics_2d_server_sw.cpp
+++ b/servers/physics_2d/physics_2d_server_sw.cpp
@@ -463,6 +463,24 @@ Matrix32 Physics2DServerSW::area_get_transform(RID p_area) const {
 	return area->get_transform();
 };
 
+void Physics2DServerSW::area_set_pickable(RID p_area,bool p_pickable) {
+
+	Area2DSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+	area->set_pickable(p_pickable);
+
+}
+
+void Physics2DServerSW::area_set_monitorable(RID p_area,bool p_monitorable) {
+
+	Area2DSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+
+	area->set_monitorable(p_monitorable);
+
+}
+
+
 void Physics2DServerSW::area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method) {
 
 	Area2DSW *area = area_owner.get(p_area);
@@ -473,6 +491,14 @@ void Physics2DServerSW::area_set_monitor_callback(RID p_area,Object *p_receiver,
 
 }
 
+void Physics2DServerSW::area_set_area_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method) {
+
+
+	Area2DSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+
+	area->set_area_monitor_callback(p_receiver?p_receiver->get_instance_ID():0,p_method);
+}
 
 /* BODY API */
 
@@ -925,6 +951,13 @@ bool Physics2DServerSW::body_collide_shape(RID p_body, int p_body_shape, RID p_s
 
 }
 
+void Physics2DServerSW::body_set_pickable(RID p_body,bool p_pickable) {
+
+	Body2DSW *body = body_owner.get(p_body);
+	ERR_FAIL_COND(!body);
+	body->set_pickable(p_pickable);
+
+}
 
 /* JOINT API */
 
diff --git a/servers/physics_2d/physics_2d_server_sw.h b/servers/physics_2d/physics_2d_server_sw.h
index e9c499aaff..58fc4eeb33 100644
--- a/servers/physics_2d/physics_2d_server_sw.h
+++ b/servers/physics_2d/physics_2d_server_sw.h
@@ -133,8 +133,12 @@ public:
 
 	virtual Variant area_get_param(RID p_parea,AreaParameter p_param) const;
 	virtual Matrix32 area_get_transform(RID p_area) const;
+	virtual void area_set_monitorable(RID p_area,bool p_monitorable);
 
 	virtual void area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method);
+	virtual void area_set_area_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method);
+
+	virtual void area_set_pickable(RID p_area,bool p_pickable);
 
 
 	/* BODY API */
@@ -217,6 +221,8 @@ public:
 	virtual void body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata=Variant());
 	virtual bool body_collide_shape(RID p_body, int p_body_shape,RID p_shape, const Matrix32& p_shape_xform,const Vector2& p_motion,Vector2 *r_results,int p_result_max,int &r_result_count);
 
+	virtual void body_set_pickable(RID p_body,bool p_pickable);
+
 	/* JOINT API */
 
 	virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value);
diff --git a/servers/physics_2d/shape_2d_sw.cpp b/servers/physics_2d/shape_2d_sw.cpp
index ed63870a12..9a4b52d563 100644
--- a/servers/physics_2d/shape_2d_sw.cpp
+++ b/servers/physics_2d/shape_2d_sw.cpp
@@ -106,6 +106,11 @@ void LineShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int
 	r_amount=0;
 }
 
+bool LineShape2DSW::contains_point(const Vector2& p_point) const {
+
+	return normal.dot(p_point) < d;
+}
+
 bool LineShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
 
 	Vector2 segment= p_begin - p_end;
@@ -175,6 +180,11 @@ void RayShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,int
 
 }
 
+bool RayShape2DSW::contains_point(const Vector2& p_point) const {
+
+	return false;
+}
+
 bool RayShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
 
 	return false; //rays can't be intersected
@@ -223,6 +233,11 @@ void SegmentShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,
 
 }
 
+bool SegmentShape2DSW::contains_point(const Vector2& p_point) const {
+
+	return false;
+}
+
 bool SegmentShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
 
 	if (!Geometry::segment_intersects_segment_2d(p_begin,p_end,a,b,&r_point))
@@ -288,6 +303,13 @@ void CircleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,i
 
 }
 
+
+bool CircleShape2DSW::contains_point(const Vector2& p_point) const {
+
+	return p_point.length_squared() < radius*radius;
+}
+
+
 bool CircleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
 
 
@@ -375,6 +397,11 @@ void RectangleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_support
 
 }
 
+bool RectangleShape2DSW::contains_point(const Vector2& p_point) const {
+
+	return Math::abs(p_point.x)<half_extents.x && Math::abs(p_point.y)<half_extents.y;
+}
+
 bool RectangleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
 
 
@@ -439,6 +466,17 @@ void CapsuleShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_supports,
 	}
 }
 
+bool CapsuleShape2DSW::contains_point(const Vector2& p_point) const {
+
+	Vector2 p = p_point;
+	p.y=Math::abs(p.y);
+	p.y-=height*0.5;
+	if (p.y<0)
+		p.y=0;
+
+	return p.length_squared() < radius*radius;
+}
+
 bool CapsuleShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
 
 
@@ -574,6 +612,25 @@ void ConvexPolygonShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_sup
 
 }
 
+
+bool ConvexPolygonShape2DSW::contains_point(const Vector2& p_point) const {
+
+	bool out=false;
+	bool in=false;
+
+	for(int i=0;i<point_count;i++) {
+
+		float d = points[i].normal.dot(p_point) - points[i].normal.dot(points[i].pos);
+		if (d>0)
+			out=true;
+		else
+			in=true;
+	}
+
+	return (in && !out) || (!in && out);
+}
+
+
 bool ConvexPolygonShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const {
 
 	Vector2 n = (p_end-p_begin).normalized();
@@ -734,6 +791,12 @@ void ConcavePolygonShape2DSW::get_supports(const Vector2& p_normal,Vector2 *r_su
 
 }
 
+bool ConcavePolygonShape2DSW::contains_point(const Vector2& p_point) const {
+
+	return false; //sorry
+}
+
+
 bool ConcavePolygonShape2DSW::intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const{
 
 	uint32_t* stack = (uint32_t*)alloca(sizeof(int)*bvh_depth);
diff --git a/servers/physics_2d/shape_2d_sw.h b/servers/physics_2d/shape_2d_sw.h
index 931491efd5..05ea5b21cd 100644
--- a/servers/physics_2d/shape_2d_sw.h
+++ b/servers/physics_2d/shape_2d_sw.h
@@ -78,6 +78,8 @@ public:
 
 	virtual bool is_concave() const { return false; }
 
+	virtual bool contains_point(const Vector2& p_point) const=0;
+
 	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const=0;
 	virtual void project_range_castv(const Vector2& p_cast, const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const=0;
 	virtual Vector2 get_support(const Vector2& p_normal) const;
@@ -171,6 +173,7 @@ public:
 	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
 	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
 
+	virtual bool contains_point(const Vector2& p_point) const;
 	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
 	virtual real_t get_moment_of_inertia(float p_mass,const Vector2& p_scale) const;
 
@@ -213,6 +216,7 @@ public:
 	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
 	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
 
+	virtual bool contains_point(const Vector2& p_point) const;
 	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
 	virtual real_t get_moment_of_inertia(float p_mass,const Vector2& p_scale) const;
 
@@ -260,6 +264,7 @@ public:
 	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
 	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
 
+	virtual bool contains_point(const Vector2& p_point) const;
 	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
 	virtual real_t get_moment_of_inertia(float p_mass,const Vector2& p_scale) const;
 
@@ -297,6 +302,7 @@ public:
 	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
 	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
 
+	virtual bool contains_point(const Vector2& p_point) const;
 	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
 	virtual real_t get_moment_of_inertia(float p_mass,const Vector2& p_scale) const;
 
@@ -336,6 +342,7 @@ public:
 	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
 	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
 
+	virtual bool contains_point(const Vector2& p_point) const;
 	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
 	virtual real_t get_moment_of_inertia(float p_mass,const Vector2& p_scale) const;
 
@@ -423,6 +430,7 @@ public:
 	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
 	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
 
+	virtual bool contains_point(const Vector2& p_point) const;
 	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
 	virtual real_t get_moment_of_inertia(float p_mass,const Vector2& p_scale) const;
 
@@ -485,6 +493,7 @@ public:
 	virtual void project_rangev(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { project_range(p_normal,p_transform,r_min,r_max); }
 	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
 
+	virtual bool contains_point(const Vector2& p_point) const;
 	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
 	virtual real_t get_moment_of_inertia(float p_mass,const Vector2& p_scale) const;
 
@@ -572,6 +581,7 @@ public:
 	virtual void project_range(const Vector2& p_normal, const Matrix32& p_transform, real_t &r_min, real_t &r_max) const { /*project_range(p_normal,p_transform,r_min,r_max);*/ }
 	virtual void get_supports(const Vector2& p_normal,Vector2 *r_supports,int & r_amount) const;
 
+	virtual bool contains_point(const Vector2& p_point) const;
 	virtual bool intersect_segment(const Vector2& p_begin,const Vector2& p_end,Vector2 &r_point, Vector2 &r_normal) const;
 
 	virtual real_t get_moment_of_inertia(float p_mass,const Vector2& p_scale) const { return 0; }
diff --git a/servers/physics_2d/space_2d_sw.cpp b/servers/physics_2d/space_2d_sw.cpp
index f2ed74ffbf..5aaf9a7613 100644
--- a/servers/physics_2d/space_2d_sw.cpp
+++ b/servers/physics_2d/space_2d_sw.cpp
@@ -45,6 +45,57 @@ _FORCE_INLINE_ static bool _match_object_type_query(CollisionObject2DSW *p_objec
 
 }
 
+
+int Physics2DDirectSpaceStateSW::intersect_point(const Vector2& p_point,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask) {
+
+	if (p_result_max<=0)
+		return 0;
+
+	Rect2 aabb;
+	aabb.pos=p_point-Vector2(0.00001,0.00001);
+	aabb.size=Vector2(0.00002,0.00002);
+
+	int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,Space2DSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
+
+	int cc=0;
+
+	for(int i=0;i<amount;i++) {
+
+		if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
+			continue;
+
+		if (p_exclude.has( space->intersection_query_results[i]->get_self()))
+			continue;
+
+		const CollisionObject2DSW *col_obj=space->intersection_query_results[i];
+
+		if (!col_obj->is_pickable())
+			continue;
+
+		int shape_idx=space->intersection_query_subindex_results[i];
+
+		Shape2DSW * shape = col_obj->get_shape(shape_idx);
+
+		Vector2 local_point = (col_obj->get_transform() * col_obj->get_shape_transform(shape_idx)).affine_inverse().xform(p_point);
+
+		if (!shape->contains_point(local_point))
+			continue;
+
+		r_results[cc].collider_id=col_obj->get_instance_id();
+		if (r_results[cc].collider_id!=0)
+			r_results[cc].collider=ObjectDB::get_instance(r_results[cc].collider_id);
+		r_results[cc].rid=col_obj->get_self();
+		r_results[cc].shape=shape_idx;
+		r_results[cc].metadata=col_obj->get_shape_metadata(shape_idx);
+
+		cc++;
+	}
+
+	return cc;
+
+
+}
+
 bool Physics2DDirectSpaceStateSW::intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude,uint32_t p_layer_mask,uint32_t p_object_type_mask) {
 
 
@@ -527,13 +578,20 @@ void* Space2DSW::_broadphase_pair(CollisionObject2DSW *A,int p_subindex_A,Collis
 
 	if (type_A==CollisionObject2DSW::TYPE_AREA) {
 
-		ERR_FAIL_COND_V(type_B!=CollisionObject2DSW::TYPE_BODY,NULL);
 		Area2DSW *area=static_cast<Area2DSW*>(A);
-		Body2DSW *body=static_cast<Body2DSW*>(B);
+		if (type_B==CollisionObject2DSW::TYPE_AREA) {
+
+			Area2DSW *area_b=static_cast<Area2DSW*>(B);
+			Area2Pair2DSW *area2_pair = memnew(Area2Pair2DSW(area_b,p_subindex_B,area,p_subindex_A) );
+			return area2_pair;
+		} else {
+
+			Body2DSW *body=static_cast<Body2DSW*>(B);
+			AreaPair2DSW *area_pair = memnew(AreaPair2DSW(body,p_subindex_B,area,p_subindex_A) );
+			return area_pair;
+		}
 
-		AreaPair2DSW *area_pair = memnew(AreaPair2DSW(body,p_subindex_B,area,p_subindex_A) );
 
-		return area_pair;
 	} else {
 
 
diff --git a/servers/physics_2d/space_2d_sw.h b/servers/physics_2d/space_2d_sw.h
index 7977b19063..05b55fe807 100644
--- a/servers/physics_2d/space_2d_sw.h
+++ b/servers/physics_2d/space_2d_sw.h
@@ -46,6 +46,7 @@ public:
 
 	Space2DSW *space;
 
+	virtual int intersect_point(const Vector2& p_point,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
 	virtual bool intersect_ray(const Vector2& p_from, const Vector2& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
 	virtual int intersect_shape(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
 	virtual bool cast_motion(const RID& p_shape, const Matrix32& p_xform,const Vector2& p_motion,float p_margin,float &p_closest_safe,float &p_closest_unsafe, const Set<RID>& p_exclude=Set<RID>(),uint32_t p_layer_mask=0xFFFFFFFF,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);