8 files changed, 232 insertions, 11 deletions

diff --git a/servers/physics/area_pair_sw.cpp b/servers/physics/area_pair_sw.cpp
index e009bd52cb..3eb96fb681 100644
--- a/servers/physics/area_pair_sw.cpp
+++ b/servers/physics/area_pair_sw.cpp
@@ -93,3 +93,72 @@ AreaPairSW::~AreaPairSW() {
 	body->remove_constraint(this);
 	area->remove_constraint(this);
 }
+
+////////////////////////////////////////////////////
+
+
+
+bool Area2PairSW::setup(float p_step) {
+
+//	bool result = area_a->test_collision_mask(area_b) && CollisionSolverSW::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);
+	bool result = CollisionSolverSW::solve_static(area_a->get_shape(shape_a),area_a->get_transform() * area_a->get_shape_transform(shape_a),area_b->get_shape(shape_b),area_b->get_transform() * area_b->get_shape_transform(shape_b),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 Area2PairSW::solve(float p_step) {
+
+
+}
+
+
+Area2PairSW::Area2PairSW(AreaSW *p_area_a,int p_shape_a, AreaSW *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);
+
+}
+
+Area2PairSW::~Area2PairSW() {
+
+	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/area_pair_sw.h b/servers/physics/area_pair_sw.h
index 1c219a05fd..4f8087280a 100644
--- a/servers/physics/area_pair_sw.h
+++ b/servers/physics/area_pair_sw.h
@@ -49,5 +49,23 @@ public:
 	~AreaPairSW();
 };
 
+
+class Area2PairSW : public ConstraintSW {
+
+	AreaSW *area_a;
+	AreaSW *area_b;
+	int shape_a;
+	int shape_b;
+	bool colliding;
+public:
+
+	bool setup(float p_step);
+	void solve(float p_step);
+
+	Area2PairSW(AreaSW *p_area_a,int p_shape_a, AreaSW *p_area_b,int p_shape_b);
+	~Area2PairSW();
+};
+
+
 #endif // AREA_PAIR__SW_H
 
diff --git a/servers/physics/area_sw.cpp b/servers/physics/area_sw.cpp
index de241c948b..83be80f816 100644
--- a/servers/physics/area_sw.cpp
+++ b/servers/physics/area_sw.cpp
@@ -31,6 +31,7 @@
 #include "body_sw.h"
 
 AreaSW::BodyKey::BodyKey(BodySW *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; }
+AreaSW::BodyKey::BodyKey(AreaSW *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 AreaSW::_shapes_changed() {
 
@@ -57,6 +58,7 @@ void AreaSW::set_space(SpaceSW *p_space) {
 	}
 
 	monitored_bodies.clear();
+	monitored_areas.clear();
 
 	_set_space(p_space);
 }
@@ -76,11 +78,32 @@ void AreaSW::set_monitor_callback(ObjectID p_id, const StringName& p_method) {
 	monitor_callback_method=p_method;
 
 	monitored_bodies.clear();
+	monitored_areas.clear();
+
 
 	_shape_changed();
 
 }
 
+void AreaSW::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 AreaSW::set_space_override_mode(PhysicsServer::AreaSpaceOverrideMode p_mode) {
@@ -98,6 +121,7 @@ void AreaSW::set_param(PhysicsServer::AreaParameter p_param, const Variant& p_va
 		case PhysicsServer::AREA_PARAM_GRAVITY: gravity=p_value; ; break;
 		case PhysicsServer::AREA_PARAM_GRAVITY_VECTOR: gravity_vector=p_value; ; break;
 		case PhysicsServer::AREA_PARAM_GRAVITY_IS_POINT: gravity_is_point=p_value; ; break;
+		case PhysicsServer::AREA_PARAM_GRAVITY_DISTANCE_SCALE: gravity_distance_scale=p_value; ; break;
 		case PhysicsServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION:  point_attenuation=p_value; ; break;
 		case PhysicsServer::AREA_PARAM_DENSITY: density=p_value; ; break;
 		case PhysicsServer::AREA_PARAM_PRIORITY: priority=p_value; ; break;
@@ -113,6 +137,7 @@ Variant AreaSW::get_param(PhysicsServer::AreaParameter p_param) const {
 		case PhysicsServer::AREA_PARAM_GRAVITY: return gravity;
 		case PhysicsServer::AREA_PARAM_GRAVITY_VECTOR: return gravity_vector;
 		case PhysicsServer::AREA_PARAM_GRAVITY_IS_POINT: return gravity_is_point;
+		case PhysicsServer::AREA_PARAM_GRAVITY_DISTANCE_SCALE: return gravity_distance_scale;
 		case PhysicsServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION: return  point_attenuation;
 		case PhysicsServer::AREA_PARAM_DENSITY: return density;
 		case PhysicsServer::AREA_PARAM_PRIORITY: return priority;
@@ -132,6 +157,15 @@ void AreaSW::_queue_monitor_update() {
 
 }
 
+void AreaSW::set_monitorable(bool p_monitorable) {
+
+	if (monitorable==p_monitorable)
+		return;
+
+	monitorable=p_monitorable;
+	_set_static(!monitorable);
+}
+
 void AreaSW::call_queries() {
 
 	if (monitor_callback_id && !monitored_bodies.empty()) {
@@ -166,6 +200,41 @@ void AreaSW::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 ? PhysicsServer::AREA_BODY_ADDED : PhysicsServer::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);
 
 }
@@ -177,11 +246,14 @@ AreaSW::AreaSW() : CollisionObjectSW(TYPE_AREA), monitor_query_list(this),  move
 	gravity=9.80665;
 	gravity_vector=Vector3(0,-1,0);
 	gravity_is_point=false;
+	gravity_distance_scale=0;
 	point_attenuation=1;
 	density=0.1;
 	priority=0;
 	set_ray_pickable(false);
 	monitor_callback_id=0;
+	area_monitor_callback_id=0;
+	monitorable=false;
 
 }
 
diff --git a/servers/physics/area_sw.h b/servers/physics/area_sw.h
index 87843b93c9..26d6a177db 100644
--- a/servers/physics/area_sw.h
+++ b/servers/physics/area_sw.h
@@ -45,13 +45,18 @@ class AreaSW : public CollisionObjectSW{
 	float gravity;
 	Vector3 gravity_vector;
 	bool gravity_is_point;
+	float gravity_distance_scale;
 	float point_attenuation;
 	float density;
 	int priority;
+	bool monitorable;
 
 	ObjectID monitor_callback_id;
 	StringName monitor_callback_method;
 
+	ObjectID area_monitor_callback_id;
+	StringName area_monitor_callback_method;
+
 	SelfList<AreaSW> monitor_query_list;
 	SelfList<AreaSW> moved_list;
 
@@ -78,6 +83,8 @@ class AreaSW : public CollisionObjectSW{
 
 		_FORCE_INLINE_ BodyKey() {}
 		BodyKey(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape);
+		BodyKey(AreaSW *p_body, uint32_t p_body_shape,uint32_t p_area_shape);
+
 	};
 
 	struct BodyState {
@@ -89,6 +96,7 @@ class AreaSW : public CollisionObjectSW{
 	};
 
 	Map<BodyKey,BodyState> monitored_bodies;
+	Map<BodyKey,BodyState> monitored_areas;
 
 	//virtual void shape_changed_notify(ShapeSW *p_shape);
 	//virtual void shape_deleted_notify(ShapeSW *p_shape);
@@ -107,9 +115,15 @@ 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(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape);
 	_FORCE_INLINE_ void remove_body_from_query(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape);
 
+	_FORCE_INLINE_ void add_area_to_query(AreaSW *p_area, uint32_t p_area_shape,uint32_t p_self_shape);
+	_FORCE_INLINE_ void remove_area_from_query(AreaSW *p_area, uint32_t p_area_shape,uint32_t p_self_shape);
+
 	void set_param(PhysicsServer::AreaParameter p_param, const Variant& p_value);
 	Variant get_param(PhysicsServer::AreaParameter p_param) const;
 
@@ -125,6 +139,9 @@ public:
 	_FORCE_INLINE_ void set_gravity_as_point(bool p_enable) { gravity_is_point=p_enable; }
 	_FORCE_INLINE_ bool is_gravity_point() const { return gravity_is_point; }
 
+	_FORCE_INLINE_ void set_gravity_distance_scale(float scale) { gravity_distance_scale=scale; }
+	_FORCE_INLINE_ float get_gravity_distance_scale() const { return gravity_distance_scale; }
+
 	_FORCE_INLINE_ void set_point_attenuation(float p_point_attenuation) { point_attenuation=p_point_attenuation; }
 	_FORCE_INLINE_ float get_point_attenuation() const { return point_attenuation; }
 
@@ -138,8 +155,8 @@ public:
 	_FORCE_INLINE_ void remove_constraint( ConstraintSW* p_constraint) { constraints.erase(p_constraint); }
 	_FORCE_INLINE_ const Set<ConstraintSW*>& get_constraints() const { return constraints; }
 
-
-
+	void set_monitorable(bool p_monitorable);
+	_FORCE_INLINE_ bool is_monitorable() const { return monitorable; }
 
 	void set_transform(const Transform& p_transform);
 
@@ -168,6 +185,26 @@ void AreaSW::remove_body_from_query(BodySW *p_body, uint32_t p_body_shape,uint32
 }
 
 
+void AreaSW::add_area_to_query(AreaSW *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 AreaSW::remove_area_from_query(AreaSW *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/body_sw.cpp b/servers/physics/body_sw.cpp
index 1cc0b763a4..5a528ecf94 100644
--- a/servers/physics/body_sw.cpp
+++ b/servers/physics/body_sw.cpp
@@ -362,9 +362,12 @@ void BodySW::set_space(SpaceSW *p_space){
 void BodySW::_compute_area_gravity(const AreaSW *p_area) {
 
 	if (p_area->is_gravity_point()) {
-
-		gravity += (p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin()).normalized() * p_area->get_gravity();
-
+		if(p_area->get_gravity_distance_scale() > 0) {
+			Vector3 v = p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin();
+			gravity += v.normalized() * (p_area->get_gravity() / Math::pow(v.length() * p_area->get_gravity_distance_scale()+1, 2) );
+		} else {
+			gravity += (p_area->get_transform().xform(p_area->get_gravity_vector()) - get_transform().get_origin()).normalized() * p_area->get_gravity();
+		}
 	} else {
 		gravity += p_area->get_gravity_vector() * p_area->get_gravity();
 	}
diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp
index 03d5b7afa1..521ffae0ea 100644
--- a/servers/physics/physics_server_sw.cpp
+++ b/servers/physics/physics_server_sw.cpp
@@ -394,6 +394,14 @@ Transform PhysicsServerSW::area_get_transform(RID p_area) const {
 	return area->get_transform();
 };
 
+void PhysicsServerSW::area_set_monitorable(RID p_area,bool p_monitorable) {
+
+	AreaSW *area = area_owner.get(p_area);
+	ERR_FAIL_COND(!area);
+
+	area->set_monitorable(p_monitorable);
+}
+
 void PhysicsServerSW::area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method) {
 
 	AreaSW *area = area_owner.get(p_area);
@@ -423,6 +431,14 @@ bool PhysicsServerSW::area_is_ray_pickable(RID p_area) const{
 }
 
 
+void PhysicsServerSW::area_set_area_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method) {
+
+
+	AreaSW *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 */
 
diff --git a/servers/physics/physics_server_sw.h b/servers/physics/physics_server_sw.h
index de2a24a378..80007b8499 100644
--- a/servers/physics/physics_server_sw.h
+++ b/servers/physics/physics_server_sw.h
@@ -128,7 +128,10 @@ public:
 	virtual void area_set_ray_pickable(RID p_area,bool p_enable);
 	virtual bool area_is_ray_pickable(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);
 
 
 	/* BODY API */
diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp
index a7465c57b9..d329a10f04 100644
--- a/servers/physics/space_sw.cpp
+++ b/servers/physics/space_sw.cpp
@@ -503,15 +503,18 @@ void* SpaceSW::_broadphase_pair(CollisionObjectSW *A,int p_subindex_A,CollisionO
 

 	if (type_A==CollisionObjectSW::TYPE_AREA) {

 

-

-		ERR_FAIL_COND_V(type_B!=CollisionObjectSW::TYPE_BODY,NULL);

 		AreaSW *area=static_cast<AreaSW*>(A);

-		BodySW *body=static_cast<BodySW*>(B);

-

+		if (type_B==CollisionObjectSW::TYPE_AREA) {

 

-		AreaPairSW *area_pair = memnew(AreaPairSW(body,p_subindex_B,area,p_subindex_A) );

+			AreaSW *area_b=static_cast<AreaSW*>(B);

+			Area2PairSW *area2_pair = memnew(Area2PairSW(area_b,p_subindex_B,area,p_subindex_A) );

+			return area2_pair;

+		} else {

 

-		return area_pair;

+			BodySW *body=static_cast<BodySW*>(B);

+			AreaPairSW *area_pair = memnew(AreaPairSW(body,p_subindex_B,area,p_subindex_A) );

+			return area_pair;

+		}

 	} else {