Ability to visually debug geometry visually:

-Visible 2D and 3D Shapes, Polygons, Tile collisions, etc.
-Visible Navmesh and Navpoly
-Visible collision contacts for 2D and 3D as a red point
-Customizable colors in project settings

31 files changed, 503 insertions, 11 deletions

diff --git a/scene/resources/box_shape.cpp b/scene/resources/box_shape.cpp
index 22ee9f8c09..ba29a1b601 100644
--- a/scene/resources/box_shape.cpp
+++ b/scene/resources/box_shape.cpp
@@ -30,6 +30,25 @@
 #include "servers/physics_server.h"
 
 
+Vector<Vector3> BoxShape::_gen_debug_mesh_lines() {
+
+
+	Vector<Vector3> lines;
+	AABB aabb;
+	aabb.pos=-get_extents();
+	aabb.size=aabb.pos*-2;
+
+	for(int i=0;i<12;i++) {
+		Vector3 a,b;
+		aabb.get_edge(i,a,b);
+		lines.push_back(a);
+		lines.push_back(b);
+	}
+
+
+	return lines;
+}
+
 void BoxShape::_update_shape() {
 
 	PhysicsServer::get_singleton()->shape_set_data(get_shape(),extents);
diff --git a/scene/resources/box_shape.h b/scene/resources/box_shape.h
index 18c5f5e5fb..9667515657 100644
--- a/scene/resources/box_shape.h
+++ b/scene/resources/box_shape.h
@@ -41,6 +41,7 @@ protected:
 	static void _bind_methods();
 
 	virtual void _update_shape();
+	virtual Vector<Vector3> _gen_debug_mesh_lines();
 
 public:
 
diff --git a/scene/resources/capsule_shape.cpp b/scene/resources/capsule_shape.cpp
index 2633b132cf..67ceed6be0 100644
--- a/scene/resources/capsule_shape.cpp
+++ b/scene/resources/capsule_shape.cpp
@@ -30,6 +30,46 @@
 #include "servers/physics_server.h"
 
 
+Vector<Vector3> CapsuleShape::_gen_debug_mesh_lines() {
+
+
+	float radius = get_radius();
+	float height = get_height();
+
+
+	Vector<Vector3> points;
+
+	Vector3 d(0,0,height*0.5);
+	for(int i=0;i<360;i++) {
+
+		float ra=Math::deg2rad(i);
+		float rb=Math::deg2rad(i+1);
+		Point2 a = Vector2(Math::sin(ra),Math::cos(ra))*radius;
+		Point2 b = Vector2(Math::sin(rb),Math::cos(rb))*radius;
+
+		points.push_back(Vector3(a.x,a.y,0)+d);
+		points.push_back(Vector3(b.x,b.y,0)+d);
+
+		points.push_back(Vector3(a.x,a.y,0)-d);
+		points.push_back(Vector3(b.x,b.y,0)-d);
+
+		if (i%90==0) {
+
+			points.push_back(Vector3(a.x,a.y,0)+d);
+			points.push_back(Vector3(a.x,a.y,0)-d);
+		}
+
+		Vector3 dud = i<180?d:-d;
+
+		points.push_back(Vector3(0,a.y,a.x)+dud);
+		points.push_back(Vector3(0,b.y,b.x)+dud);
+		points.push_back(Vector3(a.y,0,a.x)+dud);
+		points.push_back(Vector3(b.y,0,b.x)+dud);
+
+	}
+
+	return points;
+}
 
 void CapsuleShape::_update_shape() {
 
@@ -65,7 +105,6 @@ float CapsuleShape::get_height() const {
 	return height;
 }
 
-
 void CapsuleShape::_bind_methods() {
 
 	ObjectTypeDB::bind_method(_MD("set_radius","radius"),&CapsuleShape::set_radius);
diff --git a/scene/resources/capsule_shape.h b/scene/resources/capsule_shape.h
index 43b8331949..e516d0e3c7 100644
--- a/scene/resources/capsule_shape.h
+++ b/scene/resources/capsule_shape.h
@@ -43,6 +43,7 @@ protected:
 
 	virtual void _update_shape();
 
+	virtual Vector<Vector3> _gen_debug_mesh_lines();
 public:
 
 	void set_radius(float p_radius);
@@ -50,6 +51,7 @@ public:
 	void set_height(float p_height);
 	float get_height() const;
 
+
 	CapsuleShape();
 };
 
diff --git a/scene/resources/capsule_shape_2d.cpp b/scene/resources/capsule_shape_2d.cpp
index 135eb41d8f..923a509ad5 100644
--- a/scene/resources/capsule_shape_2d.cpp
+++ b/scene/resources/capsule_shape_2d.cpp
@@ -29,6 +29,7 @@
 #include "capsule_shape_2d.h"
 
 #include "servers/physics_2d_server.h"
+#include "servers/visual_server.h"
 
 void CapsuleShape2D::_update_shape() {
 
@@ -60,6 +61,32 @@ real_t CapsuleShape2D::get_height() const {
 }
 
 
+void CapsuleShape2D::draw(const RID& p_to_rid,const Color& p_color) {
+
+	Vector<Vector2> points;
+	for(int i=0;i<24;i++) {
+		Vector2 ofs = Vector2(0,(i>6 && i<=18) ? -get_height()*0.5 : get_height()*0.5);
+
+		points.push_back(Vector2(Math::sin(i*Math_PI*2/24.0),Math::cos(i*Math_PI*2/24.0))*get_radius() + ofs);
+		if (i==6 || i==18)
+			points.push_back(Vector2(Math::sin(i*Math_PI*2/24.0),Math::cos(i*Math_PI*2/24.0))*get_radius() - ofs);
+	}
+
+	Vector<Color> col;
+	col.push_back(p_color);
+	VisualServer::get_singleton()->canvas_item_add_polygon(p_to_rid,points,col);
+
+}
+
+Rect2 CapsuleShape2D::get_rect() const {
+
+	Vector2 he=Point2(get_radius(),get_radius()+get_height()*0.5);
+	Rect2 rect;
+	rect.pos=-he;
+	rect.size=he*2.0;
+	return rect;
+}
+
 void CapsuleShape2D::_bind_methods() {
 
 	ObjectTypeDB::bind_method(_MD("set_radius","radius"),&CapsuleShape2D::set_radius);
diff --git a/scene/resources/capsule_shape_2d.h b/scene/resources/capsule_shape_2d.h
index 4dd6348282..dc679966f9 100644
--- a/scene/resources/capsule_shape_2d.h
+++ b/scene/resources/capsule_shape_2d.h
@@ -49,6 +49,9 @@ public:
 	void set_radius(real_t p_radius);
 	real_t get_radius() const;
 
+	virtual void draw(const RID& p_to_rid,const Color& p_color);
+	virtual Rect2 get_rect() const ;
+
 	CapsuleShape2D();
 };
 
diff --git a/scene/resources/circle_shape_2d.cpp b/scene/resources/circle_shape_2d.cpp
index 4135bc7c3b..c77395612c 100644
--- a/scene/resources/circle_shape_2d.cpp
+++ b/scene/resources/circle_shape_2d.cpp
@@ -29,7 +29,7 @@
 #include "circle_shape_2d.h"
 
 #include "servers/physics_2d_server.h"
-
+#include "servers/visual_server.h"
 void CircleShape2D::_update_shape() {
 
 	Physics2DServer::get_singleton()->shape_set_data(get_rid(),radius);
@@ -58,6 +58,27 @@ void CircleShape2D::_bind_methods() {
 
 }
 
+Rect2 CircleShape2D::get_rect() const {
+	Rect2 rect;
+	rect.pos=-Point2(get_radius(),get_radius());
+	rect.size=Point2(get_radius(),get_radius())*2.0;
+	return rect;
+}
+
+void CircleShape2D::draw(const RID& p_to_rid,const Color& p_color) {
+
+	Vector<Vector2> points;
+	for(int i=0;i<24;i++) {
+
+		points.push_back(Vector2(Math::cos(i*Math_PI*2/24.0),Math::sin(i*Math_PI*2/24.0))*get_radius());
+	}
+
+	Vector<Color> col;
+	col.push_back(p_color);
+	VisualServer::get_singleton()->canvas_item_add_polygon(p_to_rid,points,col);
+
+}
+
 CircleShape2D::CircleShape2D() : Shape2D( Physics2DServer::get_singleton()->shape_create(Physics2DServer::SHAPE_CIRCLE)) {
 
 	radius=10;
diff --git a/scene/resources/circle_shape_2d.h b/scene/resources/circle_shape_2d.h
index d937af2979..a5902b189c 100644
--- a/scene/resources/circle_shape_2d.h
+++ b/scene/resources/circle_shape_2d.h
@@ -44,6 +44,10 @@ public:
 	void set_radius(real_t p_radius);
 	real_t get_radius() const;
 
+	virtual void draw(const RID& p_to_rid,const Color& p_color);
+	virtual Rect2 get_rect() const;
+
+
 	CircleShape2D();
 };
 
diff --git a/scene/resources/concave_polygon_shape.cpp b/scene/resources/concave_polygon_shape.cpp
index 55bd5de690..7aeac04a22 100644
--- a/scene/resources/concave_polygon_shape.cpp
+++ b/scene/resources/concave_polygon_shape.cpp
@@ -30,6 +30,40 @@
 
 #include "servers/physics_server.h"
 
+Vector<Vector3> ConcavePolygonShape::_gen_debug_mesh_lines() {
+
+	Set<DrawEdge> edges;
+
+	DVector<Vector3> data=get_faces();
+	int datalen=data.size();
+	ERR_FAIL_COND_V( (datalen%3)!=0,Vector<Vector3>() );
+
+	DVector<Vector3>::Read r=data.read();
+
+	for(int i=0;i<datalen;i+=3) {
+
+		for(int j=0;j<3;j++) {
+
+			DrawEdge de(r[i+j],r[i+((j+1)%3)]);
+			edges.insert(de);
+		}
+
+	}
+
+	Vector<Vector3> points;
+	points.resize(edges.size()*2);
+	int idx=0;
+	for (Set<DrawEdge>::Element*E=edges.front();E;E=E->next()) {
+
+		points[idx+0]=E->get().a;
+		points[idx+1]=E->get().b;
+		idx+=2;
+	}
+
+	return points;
+
+}
+
 bool ConcavePolygonShape::_set(const StringName& p_name, const Variant& p_value) {
 
 	if (p_name=="data")
diff --git a/scene/resources/concave_polygon_shape.h b/scene/resources/concave_polygon_shape.h
index 7bd80eb9c0..fae98ee046 100644
--- a/scene/resources/concave_polygon_shape.h
+++ b/scene/resources/concave_polygon_shape.h
@@ -35,16 +35,36 @@ class ConcavePolygonShape : public Shape {
 
 	OBJ_TYPE(ConcavePolygonShape,Shape);
 
+	struct DrawEdge {
+
+		Vector3 a;
+		Vector3 b;
+		bool operator<(const DrawEdge& p_edge) const {
+			if (a==p_edge.a)
+				return b<p_edge.b;
+			else
+				return a<p_edge.a;
+		}
+
+		DrawEdge(const Vector3& p_a=Vector3(),const Vector3& p_b=Vector3()) {
+			a=p_a;
+			b=p_b;
+			if (a<b) {
+				SWAP(a,b);
+			}
+		}
+	};
 
 protected:
 
+
 	bool _set(const StringName& p_name, const Variant& p_value);
 	bool _get(const StringName& p_name,Variant &r_ret) const;
 	void _get_property_list( List<PropertyInfo> *p_list) const;
 	static void _bind_methods();
 
 	virtual void _update_shape();
-
+	virtual Vector<Vector3> _gen_debug_mesh_lines();
 public:
 
 	void set_faces(const DVector<Vector3>& p_faces);
diff --git a/scene/resources/concave_polygon_shape_2d.cpp b/scene/resources/concave_polygon_shape_2d.cpp
index 0287d79dc6..923e2817ef 100644
--- a/scene/resources/concave_polygon_shape_2d.cpp
+++ b/scene/resources/concave_polygon_shape_2d.cpp
@@ -29,7 +29,7 @@
 #include "concave_polygon_shape_2d.h"
 
 #include "servers/physics_2d_server.h"
-
+#include "servers/visual_server.h"
 
 void ConcavePolygonShape2D::set_segments(const DVector<Vector2>& p_segments) {
 
@@ -41,6 +41,43 @@ DVector<Vector2> ConcavePolygonShape2D::get_segments() const {
 	return Physics2DServer::get_singleton()->shape_get_data(get_rid());
 }
 
+void ConcavePolygonShape2D::draw(const RID& p_to_rid,const Color& p_color) {
+
+
+	DVector<Vector2> s = get_segments();
+	int len=s.size();
+	if (len==0 || (len%2)==1)
+		return;
+
+	DVector<Vector2>::Read r = s.read();
+	for(int i=0;i<len;i+=2) {
+		VisualServer::get_singleton()->canvas_item_add_line(p_to_rid,r[i],r[i+1],p_color,2);
+	}
+
+}
+
+Rect2 ConcavePolygonShape2D::get_rect() const {
+
+
+	DVector<Vector2> s = get_segments();
+	int len=s.size();
+	if (len==0)
+		return Rect2();
+
+	Rect2 rect;
+
+	DVector<Vector2>::Read r = s.read();
+	for(int i=0;i<len;i++) {
+		if (i==0)
+			rect.pos=r[i];
+		else
+			rect.expand_to(r[i]);
+	}
+
+	return rect;
+
+}
+
 
 void ConcavePolygonShape2D::_bind_methods() {
 
diff --git a/scene/resources/concave_polygon_shape_2d.h b/scene/resources/concave_polygon_shape_2d.h
index b835096b2a..29666c88c1 100644
--- a/scene/resources/concave_polygon_shape_2d.h
+++ b/scene/resources/concave_polygon_shape_2d.h
@@ -41,6 +41,9 @@ public:
 	void set_segments(const DVector<Vector2>& p_segments);
 	DVector<Vector2> get_segments() const;
 
+	virtual void draw(const RID& p_to_rid,const Color& p_color);
+	virtual Rect2 get_rect() const ;
+
 	ConcavePolygonShape2D();
 };
 
diff --git a/scene/resources/convex_polygon_shape.cpp b/scene/resources/convex_polygon_shape.cpp
index 326a0e5180..6a405c9c94 100644
--- a/scene/resources/convex_polygon_shape.cpp
+++ b/scene/resources/convex_polygon_shape.cpp
@@ -28,7 +28,34 @@
 /*************************************************************************/
 #include "convex_polygon_shape.h"
 #include "servers/physics_server.h"
+#include "quick_hull.h"
 
+Vector<Vector3> ConvexPolygonShape::_gen_debug_mesh_lines() {
+
+	DVector<Vector3> points = get_points();
+
+	if (points.size()>3) {
+
+		QuickHull qh;
+		Vector<Vector3> varr = Variant(points);
+		Geometry::MeshData md;
+		Error err = qh.build(varr,md);
+		if (err==OK) {
+			Vector<Vector3> lines;
+			lines.resize(md.edges.size()*2);
+			for(int i=0;i<md.edges.size();i++) {
+				lines[i*2+0]=md.vertices[md.edges[i].a];
+				lines[i*2+1]=md.vertices[md.edges[i].b];
+			}
+			return lines;
+
+
+		}
+
+	}
+
+	return Vector<Vector3>();
+}
 
 
 void ConvexPolygonShape::_update_shape() {
diff --git a/scene/resources/convex_polygon_shape.h b/scene/resources/convex_polygon_shape.h
index e0a590e09d..48454deb2b 100644
--- a/scene/resources/convex_polygon_shape.h
+++ b/scene/resources/convex_polygon_shape.h
@@ -42,6 +42,7 @@ protected:
 
 	virtual void _update_shape();
 
+	virtual Vector<Vector3> _gen_debug_mesh_lines();
 public:
 
 	void set_points(const DVector<Vector3>& p_points);
diff --git a/scene/resources/convex_polygon_shape_2d.cpp b/scene/resources/convex_polygon_shape_2d.cpp
index 74506f8600..dac39fc846 100644
--- a/scene/resources/convex_polygon_shape_2d.cpp
+++ b/scene/resources/convex_polygon_shape_2d.cpp
@@ -29,7 +29,7 @@
 #include "convex_polygon_shape_2d.h"
 
 #include "servers/physics_2d_server.h"
-
+#include "servers/visual_server.h"
 void ConvexPolygonShape2D::_update_shape() {
 
 	Physics2DServer::get_singleton()->shape_set_data(get_rid(),points);
@@ -66,6 +66,29 @@ void ConvexPolygonShape2D::_bind_methods() {
 
 }
 
+void ConvexPolygonShape2D::draw(const RID& p_to_rid,const Color& p_color) {
+
+
+	Vector<Color> col;
+	col.push_back(p_color);
+	VisualServer::get_singleton()->canvas_item_add_polygon(p_to_rid,points,col);
+}
+
+Rect2 ConvexPolygonShape2D::get_rect() const  {
+
+
+	Rect2 rect;
+	for(int i=0;i<points.size();i++) {
+		if (i==0)
+			rect.pos=points[i];
+		else
+			rect.expand_to(points[i]);
+	}
+
+	return rect;
+
+}
+
 ConvexPolygonShape2D::ConvexPolygonShape2D() : Shape2D( Physics2DServer::get_singleton()->shape_create(Physics2DServer::SHAPE_CONVEX_POLYGON)) {
 
 
diff --git a/scene/resources/convex_polygon_shape_2d.h b/scene/resources/convex_polygon_shape_2d.h
index efcaa19be6..1af7787f67 100644
--- a/scene/resources/convex_polygon_shape_2d.h
+++ b/scene/resources/convex_polygon_shape_2d.h
@@ -45,6 +45,9 @@ public:
 	void set_points(const Vector<Vector2>& p_points);
 	Vector<Vector2> get_points() const;
 
+	virtual void draw(const RID& p_to_rid,const Color& p_color);
+	virtual Rect2 get_rect() const ;
+
 	ConvexPolygonShape2D();
 };
 
diff --git a/scene/resources/plane_shape.cpp b/scene/resources/plane_shape.cpp
index 150406ceff..760a36a91e 100644
--- a/scene/resources/plane_shape.cpp
+++ b/scene/resources/plane_shape.cpp
@@ -30,7 +30,34 @@
 
 #include "servers/physics_server.h"
 
-
+Vector<Vector3> PlaneShape::_gen_debug_mesh_lines() {
+
+	Plane p = get_plane();
+	Vector<Vector3> points;
+
+	Vector3 n1 = p.get_any_perpendicular_normal();
+	Vector3 n2 = p.normal.cross(n1).normalized();
+
+	Vector3 pface[4]={
+		p.normal*p.d+n1*10.0+n2*10.0,
+		p.normal*p.d+n1*10.0+n2*-10.0,
+		p.normal*p.d+n1*-10.0+n2*-10.0,
+		p.normal*p.d+n1*-10.0+n2*10.0,
+	};
+
+	points.push_back(pface[0]);
+	points.push_back(pface[1]);
+	points.push_back(pface[1]);
+	points.push_back(pface[2]);
+	points.push_back(pface[2]);
+	points.push_back(pface[3]);
+	points.push_back(pface[3]);
+	points.push_back(pface[0]);
+	points.push_back(p.normal*p.d);
+	points.push_back(p.normal*p.d+p.normal*3);
+
+	return points;
+}
 
 void PlaneShape::_update_shape() {
 
diff --git a/scene/resources/plane_shape.h b/scene/resources/plane_shape.h
index 3bdf8f2bef..dd285171c4 100644
--- a/scene/resources/plane_shape.h
+++ b/scene/resources/plane_shape.h
@@ -39,9 +39,9 @@ class PlaneShape : public Shape {
 protected:
 
 	static void _bind_methods();
-
 	virtual void _update_shape();
 
+	virtual Vector<Vector3> _gen_debug_mesh_lines();
 public:
 
 	void set_plane(Plane p_plane);
diff --git a/scene/resources/ray_shape.cpp b/scene/resources/ray_shape.cpp
index e12ecf107b..ee55cc6e37 100644
--- a/scene/resources/ray_shape.cpp
+++ b/scene/resources/ray_shape.cpp
@@ -30,7 +30,14 @@
 
 #include "servers/physics_server.h"
 
+Vector<Vector3> RayShape::_gen_debug_mesh_lines() {
 
+	Vector<Vector3> points;
+	points.push_back(Vector3());
+	points.push_back(Vector3(0,0,get_length()));
+
+	return points;
+}
 
 void RayShape::_update_shape() {
 
diff --git a/scene/resources/ray_shape.h b/scene/resources/ray_shape.h
index 0b6156d343..edb29b83b5 100644
--- a/scene/resources/ray_shape.h
+++ b/scene/resources/ray_shape.h
@@ -39,7 +39,7 @@ protected:
 
 	static void _bind_methods();
 	virtual void _update_shape();
-
+	virtual Vector<Vector3> _gen_debug_mesh_lines();
 public:
 
 	void set_length(float p_length);
diff --git a/scene/resources/rectangle_shape_2d.cpp b/scene/resources/rectangle_shape_2d.cpp
index d3332f45a5..9a32b013c1 100644
--- a/scene/resources/rectangle_shape_2d.cpp
+++ b/scene/resources/rectangle_shape_2d.cpp
@@ -29,7 +29,7 @@
 #include "rectangle_shape_2d.h"
 
 #include "servers/physics_2d_server.h"
-
+#include "servers/visual_server.h"
 void RectangleShape2D::_update_shape() {
 
 	Physics2DServer::get_singleton()->shape_set_data(get_rid(),extents);
@@ -48,6 +48,19 @@ Vector2 RectangleShape2D::get_extents() const {
 	return extents;
 }
 
+void RectangleShape2D::draw(const RID& p_to_rid,const Color& p_color) {
+
+
+	VisualServer::get_singleton()->canvas_item_add_rect(p_to_rid,Rect2(-extents,extents*2.0),p_color);
+
+}
+
+Rect2 RectangleShape2D::get_rect() const {
+
+	Rect2(-extents,extents*2.0);
+
+}
+
 
 void RectangleShape2D::_bind_methods() {
 
diff --git a/scene/resources/rectangle_shape_2d.h b/scene/resources/rectangle_shape_2d.h
index 0b89441d04..96de02fb70 100644
--- a/scene/resources/rectangle_shape_2d.h
+++ b/scene/resources/rectangle_shape_2d.h
@@ -44,6 +44,9 @@ public:
 	void set_extents(const Vector2& p_extents);
 	Vector2 get_extents() const;
 
+	virtual void draw(const RID& p_to_rid,const Color& p_color);
+	virtual Rect2 get_rect() const ;
+
 	RectangleShape2D();
 };
 
diff --git a/scene/resources/segment_shape_2d.cpp b/scene/resources/segment_shape_2d.cpp
index b8bd3de552..88f7adcd9b 100644
--- a/scene/resources/segment_shape_2d.cpp
+++ b/scene/resources/segment_shape_2d.cpp
@@ -29,6 +29,7 @@
 #include "segment_shape_2d.h"
 
 #include "servers/physics_2d_server.h"
+#include "servers/visual_server.h"
 
 void SegmentShape2D::_update_shape() {
 
@@ -62,6 +63,23 @@ Vector2 SegmentShape2D::get_b() const {
 	return b;
 }
 
+void SegmentShape2D::draw(const RID& p_to_rid,const Color& p_color) {
+
+	VisualServer::get_singleton()->canvas_item_add_line(p_to_rid,a,b,p_color,3);
+}
+
+Rect2 SegmentShape2D::get_rect() const{
+
+	Rect2 rect;
+	rect.pos=a;
+	rect.expand_to(b);
+	return rect;
+
+}
+
+
+
+
 void SegmentShape2D::_bind_methods() {
 
 	ObjectTypeDB::bind_method(_MD("set_a","a"),&SegmentShape2D::set_a);
@@ -94,6 +112,37 @@ void RayShape2D::_update_shape() {
 
 }
 
+
+void RayShape2D::draw(const RID& p_to_rid,const Color& p_color) {
+
+
+	Vector2 tip = Vector2(0,get_length());
+	VS::get_singleton()->canvas_item_add_line(p_to_rid,Vector2(),tip,p_color,3);
+	Vector<Vector2> pts;
+	float tsize=4;
+	pts.push_back(tip+Vector2(0,tsize));
+	pts.push_back(tip+Vector2(0.707*tsize,0));
+	pts.push_back(tip+Vector2(-0.707*tsize,0));
+	Vector<Color> cols;
+	for(int i=0;i<3;i++)
+		cols.push_back(p_color);
+
+	VS::get_singleton()->canvas_item_add_primitive(p_to_rid,pts,cols,Vector<Point2>(),RID());
+
+
+
+}
+
+Rect2 RayShape2D::get_rect() const {
+
+	Rect2 rect;
+	rect.pos=Vector2();
+	rect.expand_to(Vector2(0,length));
+	rect=rect.grow(0.707*4);
+	return rect;
+}
+
+
 void RayShape2D::_bind_methods() {
 
 	ObjectTypeDB::bind_method(_MD("set_length","length"),&RayShape2D::set_length);
diff --git a/scene/resources/segment_shape_2d.h b/scene/resources/segment_shape_2d.h
index ec771cd2ed..37c68b6c92 100644
--- a/scene/resources/segment_shape_2d.h
+++ b/scene/resources/segment_shape_2d.h
@@ -49,6 +49,9 @@ public:
 	Vector2 get_a() const;
 	Vector2 get_b() const;
 
+	virtual void draw(const RID& p_to_rid,const Color& p_color);
+	virtual Rect2 get_rect() const;
+
 	SegmentShape2D();
 };
 
@@ -67,6 +70,8 @@ public:
 
 	void set_length(real_t p_length);
 	real_t get_length() const;
+	virtual void draw(const RID& p_to_rid,const Color& p_color);
+	virtual Rect2 get_rect() const;
 
 	RayShape2D();
 };
diff --git a/scene/resources/shape.cpp b/scene/resources/shape.cpp
index 1120a8d672..143ef82d51 100644
--- a/scene/resources/shape.cpp
+++ b/scene/resources/shape.cpp
@@ -29,9 +29,67 @@
 #include "shape.h"
 
 #include "servers/physics_server.h"
+#include "scene/resources/mesh.h"
+#include "os/os.h"
+#include "scene/main/scene_main_loop.h"
 
 
+void Shape::add_vertices_to_array(DVector<Vector3> &array, const Transform& p_xform) {
 
+	Vector<Vector3> toadd = _gen_debug_mesh_lines();
+
+	if (toadd.size()) {
+
+		int base=array.size();
+		array.resize(base+toadd.size());
+		DVector<Vector3>::Write w = array.write();
+		for(int i=0;i<toadd.size();i++) {
+			w[i+base]=p_xform.xform(toadd[i]);
+		}
+
+	}
+}
+
+Ref<Mesh> Shape::get_debug_mesh() {
+
+	if (debug_mesh_cache.is_valid())
+		return debug_mesh_cache;
+
+	Vector<Vector3> lines = _gen_debug_mesh_lines();
+
+	debug_mesh_cache = Ref<Mesh>(memnew(Mesh));
+
+	if (!lines.empty()) {
+		//make mesh
+		DVector<Vector3> array;
+		array.resize(lines.size());
+		{
+
+			DVector<Vector3>::Write w=array.write();
+			for(int i=0;i<lines.size();i++) {
+				w[i]=lines[i];
+			}
+		}
+
+		Array arr;
+		arr.resize(Mesh::ARRAY_MAX);
+		arr[Mesh::ARRAY_VERTEX]=array;
+
+		SceneTree *st=OS::get_singleton()->get_main_loop()->cast_to<SceneTree>();
+
+		debug_mesh_cache->add_surface(Mesh::PRIMITIVE_LINES,arr);
+
+		if (st) {
+			debug_mesh_cache->surface_set_material(0,st->get_debug_collision_material());
+		}
+
+	}
+
+
+
+	return debug_mesh_cache;
+
+}
 
 Shape::Shape() {
 
@@ -49,3 +107,4 @@ Shape::~Shape() {
 
 	PhysicsServer::get_singleton()->free(shape);
 }
+
diff --git a/scene/resources/shape.h b/scene/resources/shape.h
index 3cc806c7a3..1992ce51c3 100644
--- a/scene/resources/shape.h
+++ b/scene/resources/shape.h
@@ -30,6 +30,7 @@
 #define SHAPE_H
 
 #include "resource.h"
+class Mesh;
 
 class Shape : public Resource {
 
@@ -38,13 +39,22 @@ class Shape : public Resource {
 	RES_BASE_EXTENSION("shp");
 	RID shape;
 
+	Ref<Mesh> debug_mesh_cache;
+
 protected:
 
 	_FORCE_INLINE_ RID get_shape() const { return shape; }
 	Shape(RID p_shape);
+
+	virtual Vector<Vector3> _gen_debug_mesh_lines()=0;// { return Vector<Vector3>(); }
 public:
 
 	virtual RID get_rid() const { return shape; }
+
+	Ref<Mesh> get_debug_mesh();
+
+	void add_vertices_to_array(DVector<Vector3> &array, const Transform& p_xform);
+
 	Shape();
 	~Shape();
 };
diff --git a/scene/resources/shape_2d.h b/scene/resources/shape_2d.h
index 55f3173db4..1737301d9d 100644
--- a/scene/resources/shape_2d.h
+++ b/scene/resources/shape_2d.h
@@ -53,6 +53,8 @@ public:
 	Variant collide_with_motion_and_get_contacts(const Matrix32& p_local_xform, const Vector2& p_local_motion, const Ref<Shape2D>& p_shape, const Matrix32& p_shape_xform, const Vector2 &p_p_shape_motion);
 	Variant collide_and_get_contacts(const Matrix32& p_local_xform,  const Ref<Shape2D>& p_shape, const Matrix32& p_shape_xform);
 
+	virtual void draw(const RID& p_to_rid,const Color& p_color) {}
+	virtual Rect2 get_rect() const { return Rect2(); }
 	virtual RID get_rid() const;
 	Shape2D();
 	~Shape2D();
diff --git a/scene/resources/shape_line_2d.cpp b/scene/resources/shape_line_2d.cpp
index d4dc46d64f..c660b604f3 100644
--- a/scene/resources/shape_line_2d.cpp
+++ b/scene/resources/shape_line_2d.cpp
@@ -28,7 +28,7 @@
 /*************************************************************************/
 #include "shape_line_2d.h"
 #include "servers/physics_2d_server.h"
-
+#include "servers/visual_server.h"
 void LineShape2D::_update_shape() {
 
 	Array arr;
@@ -61,6 +61,32 @@ real_t LineShape2D::get_d() const {
 	return d;
 }
 
+
+void LineShape2D::draw(const RID& p_to_rid,const Color& p_color) {
+
+	Vector2 point = get_d() * get_normal();
+
+	Vector2 l1[2]={point-get_normal().tangent()*100,point+get_normal().tangent()*100};
+	VS::get_singleton()->canvas_item_add_line(p_to_rid,l1[0],l1[1],p_color,3);
+	Vector2 l2[2]={point,point+get_normal()*30};
+	VS::get_singleton()->canvas_item_add_line(p_to_rid,l2[0],l2[1],p_color,3);
+
+}
+Rect2 LineShape2D::get_rect() const{
+
+	Vector2 point = get_d() * get_normal();
+
+	Vector2 l1[2]={point-get_normal().tangent()*100,point+get_normal().tangent()*100};
+	Vector2 l2[2]={point,point+get_normal()*30};
+	Rect2 rect;
+	rect.pos=l1[0];
+	rect.expand_to(l1[1]);
+	rect.expand_to(l2[0]);
+	rect.expand_to(l2[1]);
+	return rect;
+
+}
+
 void LineShape2D::_bind_methods() {
 
 	ObjectTypeDB::bind_method(_MD("set_normal","normal"),&LineShape2D::set_normal);
diff --git a/scene/resources/shape_line_2d.h b/scene/resources/shape_line_2d.h
index 3ba8f57add..f32ad7fb7c 100644
--- a/scene/resources/shape_line_2d.h
+++ b/scene/resources/shape_line_2d.h
@@ -49,6 +49,9 @@ public:
 	Vector2 get_normal() const;
 	real_t get_d() const;
 
+	virtual void draw(const RID& p_to_rid,const Color& p_color);
+	virtual Rect2 get_rect() const;
+
 	LineShape2D();
 };
 
diff --git a/scene/resources/sphere_shape.cpp b/scene/resources/sphere_shape.cpp
index e93d718efa..a7e28eb727 100644
--- a/scene/resources/sphere_shape.cpp
+++ b/scene/resources/sphere_shape.cpp
@@ -29,6 +29,30 @@
 #include "sphere_shape.h"
 #include "servers/physics_server.h"
 
+Vector<Vector3> SphereShape::_gen_debug_mesh_lines() {
+
+	float r=get_radius();
+
+	Vector<Vector3> points;
+
+	for(int i=0;i<=360;i++) {
+
+		float ra=Math::deg2rad(i);
+		float rb=Math::deg2rad(i+1);
+		Point2 a = Vector2(Math::sin(ra),Math::cos(ra))*r;
+		Point2 b = Vector2(Math::sin(rb),Math::cos(rb))*r;
+
+		points.push_back(Vector3(a.x,0,a.y));
+		points.push_back(Vector3(b.x,0,b.y));
+		points.push_back(Vector3(0,a.x,a.y));
+		points.push_back(Vector3(0,b.x,b.y));
+		points.push_back(Vector3(a.x,a.y,0));
+		points.push_back(Vector3(b.x,b.y,0));
+
+	}
+
+	return points;
+}
 
 void SphereShape::_update_shape() {
 
diff --git a/scene/resources/sphere_shape.h b/scene/resources/sphere_shape.h
index 1bbddb9fd9..2543d94439 100644
--- a/scene/resources/sphere_shape.h
+++ b/scene/resources/sphere_shape.h
@@ -42,7 +42,7 @@ protected:
 	static void _bind_methods();
 
 	virtual void _update_shape();
-
+	virtual Vector<Vector3> _gen_debug_mesh_lines();
 public:
 
 	void set_radius(float p_radius);