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

servers/physics_2d/body_pair_2d_sw.cpp

@@ -379,7 +379,12 @@ bool BodyPair2DSW::setup(float p_step) {
 		}
 
 		c.active=true;
-
+#ifdef DEBUG_ENABLED
+		if (space->is_debugging_contacts()) {
+			space->add_debug_contact(global_A+offset_A);
+			space->add_debug_contact(global_B+offset_A);
+		}
+#endif
 		int gather_A = A->can_report_contacts();
 		int gather_B = B->can_report_contacts();
 

servers/physics_2d/physics_2d_server_sw.cpp

@@ -257,6 +257,30 @@ real_t Physics2DServerSW::space_get_param(RID p_space,SpaceParameter p_param) co
 	return space->get_param(p_param);
 }
 
+void Physics2DServerSW::space_set_debug_contacts(RID p_space,int p_max_contacts) {
+
+	Space2DSW *space = space_owner.get(p_space);
+	ERR_FAIL_COND(!space);
+	space->set_debug_contacts(p_max_contacts);
+
+}
+
+Vector<Vector2> Physics2DServerSW::space_get_contacts(RID p_space) const {
+
+	Space2DSW *space = space_owner.get(p_space);
+	ERR_FAIL_COND_V(!space,Vector<Vector2>());
+	return space->get_debug_contacts();
+
+}
+
+int Physics2DServerSW::space_get_contact_count(RID p_space) const {
+
+	Space2DSW *space = space_owner.get(p_space);
+	ERR_FAIL_COND_V(!space,0);
+	return space->get_debug_contact_count();
+
+}
+
 Physics2DDirectSpaceState* Physics2DServerSW::space_get_direct_state(RID p_space) {
 
 	Space2DSW *space = space_owner.get(p_space);

servers/physics_2d/physics_2d_server_sw.h

@@ -102,6 +102,11 @@ public:
 	virtual void space_set_param(RID p_space,SpaceParameter p_param, real_t p_value);
 	virtual real_t space_get_param(RID p_space,SpaceParameter p_param) const;
 
+	virtual void space_set_debug_contacts(RID p_space,int p_max_contacts);
+	virtual Vector<Vector2> space_get_contacts(RID p_space) const;
+	virtual int space_get_contact_count(RID p_space) const;
+
+
 	// this function only works on fixed process, errors and returns null otherwise
 	virtual Physics2DDirectSpaceState* space_get_direct_state(RID p_space);
 

servers/physics_2d/physics_2d_server_wrap_mt.h

@@ -94,6 +94,22 @@ public:
 		return physics_2d_server->space_get_direct_state(p_space);
 	}
 
+	FUNC2(space_set_debug_contacts,RID,int);
+	virtual Vector<Vector2> space_get_contacts(RID p_space) const {
+
+		ERR_FAIL_COND_V(main_thread!=Thread::get_caller_ID(),Vector<Vector2>());
+		return physics_2d_server->space_get_contacts(p_space);
+
+	}
+
+	virtual int space_get_contact_count(RID p_space) const {
+
+		ERR_FAIL_COND_V(main_thread!=Thread::get_caller_ID(),0);
+		return physics_2d_server->space_get_contact_count(p_space);
+
+	}
+
+
 
 	/* AREA API */
 

servers/physics_2d/space_2d_sw.cpp

@@ -1230,6 +1230,7 @@ void Space2DSW::call_queries() {
 
 void Space2DSW::setup() {
 
+	contact_debug_count=0;
 
 	while(inertia_update_list.first()) {
 		inertia_update_list.first()->self()->update_inertias();
@@ -1302,6 +1303,8 @@ Space2DSW::Space2DSW() {
 	active_objects=0;
 	island_count=0;
 
+	contact_debug_count=0;
+
 	locked=false;
 	contact_recycle_radius=0.01;
 	contact_max_separation=0.05;
@@ -1320,6 +1323,10 @@ Space2DSW::Space2DSW() {
 
 	direct_access = memnew( Physics2DDirectSpaceStateSW );
 	direct_access->space=this;
+
+
+
+
 }
 
 Space2DSW::~Space2DSW() {

servers/physics_2d/space_2d_sw.h

@@ -103,6 +103,9 @@ class Space2DSW {
 
 	int _cull_aabb_for_body(Body2DSW *p_body,const Rect2& p_aabb);
 
+	Vector<Vector2> contact_debug;
+	int contact_debug_count;
+
 friend class Physics2DDirectSpaceStateSW;
 
 public:
@@ -169,6 +172,14 @@ public:
 
 	bool test_body_motion(Body2DSW *p_body, const Vector2&p_motion, float p_margin, Physics2DServer::MotionResult *r_result);
 
+
+	void set_debug_contacts(int p_amount) { contact_debug.resize(p_amount); }
+	_FORCE_INLINE_ bool is_debugging_contacts() const { return !contact_debug.empty(); }
+	_FORCE_INLINE_ void add_debug_contact(const Vector2& p_contact) { if (contact_debug_count<contact_debug.size()) contact_debug[contact_debug_count++]=p_contact; }
+	_FORCE_INLINE_ Vector<Vector2> get_debug_contacts() { return contact_debug; }
+	_FORCE_INLINE_ int get_debug_contact_count() { return contact_debug_count; }
+
+
 	Physics2DDirectSpaceStateSW *get_direct_state();
 
 	Space2DSW();