6 files changed, 89 insertions, 1 deletions

diff --git a/scene/3d/area.cpp b/scene/3d/area.cpp
index 407747fc0d..cb1df78fda 100644
--- a/scene/3d/area.cpp
+++ b/scene/3d/area.cpp
@@ -255,6 +255,24 @@ bool Area::is_monitoring_enabled() const {
 }
 
 
+Array Area::get_overlapping_bodies() const {
+
+	ERR_FAIL_COND_V(!monitoring,Array());
+	Array ret;
+	ret.resize(body_map.size());
+	int idx=0;
+	for (const Map<ObjectID,BodyState>::Element *E=body_map.front();E;E=E->next()) {
+		Object *obj = ObjectDB::get_instance(E->key());
+		if (!obj) {
+			ret.resize( ret.size() -1 ); //ops
+		} else {
+			ret[idx++]=obj;
+		}
+
+	}
+
+	return ret;
+}
 
 void Area::_bind_methods() {
 
@@ -283,6 +301,8 @@ void Area::_bind_methods() {
 	ObjectTypeDB::bind_method(_MD("set_enable_monitoring","enable"),&Area::set_enable_monitoring);
 	ObjectTypeDB::bind_method(_MD("is_monitoring_enabled"),&Area::is_monitoring_enabled);
 
+	ObjectTypeDB::bind_method(_MD("get_overlapping_bodies"),&Area::get_overlapping_bodies);
+
 	ObjectTypeDB::bind_method(_MD("_body_inout"),&Area::_body_inout);
 
 
diff --git a/scene/3d/area.h b/scene/3d/area.h
index 96b8585338..4707b73e1c 100644
--- a/scene/3d/area.h
+++ b/scene/3d/area.h
@@ -112,6 +112,8 @@ public:
 	void set_enable_monitoring(bool p_enable);
 	bool is_monitoring_enabled() const;
 
+	Array get_overlapping_bodies() const;
+
 
 	Area();
 	~Area();
diff --git a/scene/3d/navigation.cpp b/scene/3d/navigation.cpp
index d22198d47e..ce002fb44b 100644
--- a/scene/3d/navigation.cpp
+++ b/scene/3d/navigation.cpp
@@ -182,6 +182,41 @@ void Navigation::navmesh_remove(int p_id){
 
 }
 
+void Navigation::_clip_path(Vector<Vector3>& path, Polygon *from_poly, const Vector3& p_to_point, Polygon* p_to_poly) {
+
+	Vector3 from = path[path.size()-1];
+
+	if (from.distance_to(p_to_point)<CMP_EPSILON)
+		return;
+	Plane cut_plane;
+	cut_plane.normal = (from-p_to_point).cross(up);
+	if (cut_plane.normal==Vector3())
+		return;
+	cut_plane.normal.normalize();
+	cut_plane.d = cut_plane.normal.dot(from);
+
+
+	while(from_poly!=p_to_poly) {
+
+		int pe = from_poly->prev_edge;
+		Vector3 a = _get_vertex(from_poly->edges[pe].point);
+		Vector3 b = _get_vertex(from_poly->edges[(pe+1)%from_poly->edges.size()].point);
+
+		from_poly=from_poly->edges[pe].C;
+		ERR_FAIL_COND(!from_poly);
+
+		if (a.distance_to(b)>CMP_EPSILON) {
+
+			Vector3 inters;
+			if (cut_plane.intersects_segment(a,b,&inters)) {
+				if (inters.distance_to(p_to_point)>CMP_EPSILON && inters.distance_to(path[path.size()-1])>CMP_EPSILON) {
+					path.push_back(inters);
+				}
+			}
+		}
+	}
+}
+
 Vector<Vector3> Navigation::get_simple_path(const Vector3& p_start, const Vector3& p_end, bool p_optimize) {
 
 
@@ -379,9 +414,12 @@ Vector<Vector3> Navigation::get_simple_path(const Vector3& p_start, const Vector
 						portal_left=left;
 					} else {
 
-						apex_point=portal_right;
+						_clip_path(path,apex_poly,portal_right,right_poly);
+
+						apex_point=portal_right;						
 						p=right_poly;
 						left_poly=p;
+						apex_poly=p;
 						portal_left=apex_point;
 						portal_right=apex_point;
 						path.push_back(apex_point);
@@ -396,9 +434,12 @@ Vector<Vector3> Navigation::get_simple_path(const Vector3& p_start, const Vector
 						portal_right=right;
 					} else {
 
+						_clip_path(path,apex_poly,portal_left,left_poly);
+
 						apex_point=portal_left;
 						p=left_poly;
 						right_poly=p;
+						apex_poly=p;
 						portal_right=apex_point;
 						portal_left=apex_point;
 						path.push_back(apex_point);
diff --git a/scene/3d/navigation.h b/scene/3d/navigation.h
index 9b6cf5fbc4..69d48531a7 100644
--- a/scene/3d/navigation.h
+++ b/scene/3d/navigation.h
@@ -118,6 +118,7 @@ class Navigation : public Spatial {
 	int last_id;
 
 	Vector3 up;
+	void _clip_path(Vector<Vector3>& path,Polygon *from_poly, const Vector3& p_to_point, Polygon* p_to_poly);
 
 protected:
 
diff --git a/scene/3d/physics_body.cpp b/scene/3d/physics_body.cpp
index a80fdce64c..21ecac6e3d 100644
--- a/scene/3d/physics_body.cpp
+++ b/scene/3d/physics_body.cpp
@@ -637,6 +637,27 @@ RigidBody::AxisLock RigidBody::get_axis_lock() const {
 }
 
 
+Array RigidBody::get_colliding_bodies() const {
+
+	ERR_FAIL_COND_V(!contact_monitor,Array());
+
+	Array ret;
+	ret.resize(contact_monitor->body_map.size());
+	int idx=0;
+	for (const Map<ObjectID,BodyState>::Element *E=contact_monitor->body_map.front();E;E=E->next()) {
+		Object *obj = ObjectDB::get_instance(E->key());
+		if (!obj) {
+			ret.resize( ret.size() -1 ); //ops
+		} else {
+			ret[idx++]=obj;
+		}
+
+	}
+
+	return ret;
+}
+
+
 void RigidBody::_bind_methods() {
 
 	ObjectTypeDB::bind_method(_MD("set_mode","mode"),&RigidBody::set_mode);
@@ -688,6 +709,8 @@ void RigidBody::_bind_methods() {
 	ObjectTypeDB::bind_method(_MD("set_axis_lock","axis_lock"),&RigidBody::set_axis_lock);
 	ObjectTypeDB::bind_method(_MD("get_axis_lock"),&RigidBody::get_axis_lock);
 
+	ObjectTypeDB::bind_method(_MD("get_colliding_bodies"),&RigidBody::get_colliding_bodies);
+
 	BIND_VMETHOD(MethodInfo("_integrate_forces",PropertyInfo(Variant::OBJECT,"state:PhysicsDirectBodyState")));
 
 	ADD_PROPERTY( PropertyInfo(Variant::INT,"mode",PROPERTY_HINT_ENUM,"Rigid,Static,Character,Kinematic"),_SCS("set_mode"),_SCS("get_mode"));
diff --git a/scene/3d/physics_body.h b/scene/3d/physics_body.h
index f9f028e20d..0d1de7f236 100644
--- a/scene/3d/physics_body.h
+++ b/scene/3d/physics_body.h
@@ -236,6 +236,7 @@ public:
 	void set_axis_lock(AxisLock p_lock);
 	AxisLock get_axis_lock() const;
 
+	Array get_colliding_bodies() const;
 
 	void apply_impulse(const Vector3& p_pos, const Vector3& p_impulse);