From 93d6003aa8602262fb4878620178f51e828e5b0c Mon Sep 17 00:00:00 2001
From: Benjamin Dahse <ronoueb@gmail.com>
Date: Tue, 21 Mar 2017 17:09:04 +0100
Subject: [PATCH] Show 3D raycasts when debugging collisions

Represent a raycast by a line segment using the direction and magnitude of the cast_to vector.
The entire line segment will become red while a collision occurs.
---
 scene/3d/ray_cast.cpp | 86 +++++++++++++++++++++++++++++++++++++++++++
 scene/3d/ray_cast.h   |  7 ++++
 2 files changed, 93 insertions(+)

diff --git a/scene/3d/ray_cast.cpp b/scene/3d/ray_cast.cpp
index a73114a7c76..d5fba089e83 100644
--- a/scene/3d/ray_cast.cpp
+++ b/scene/3d/ray_cast.cpp
@@ -29,12 +29,15 @@
 #include "ray_cast.h"
 
 #include "collision_object.h"
+#include "mesh_instance.h"
 #include "servers/physics_server.h"
 void RayCast::set_cast_to(const Vector3 &p_point) {
 
 	cast_to = p_point;
 	if (is_inside_tree() && (get_tree()->is_editor_hint() || get_tree()->is_debugging_collisions_hint()))
 		update_gizmo();
+	if (is_inside_tree() && get_tree()->is_debugging_collisions_hint())
+		_update_debug_shape();
 }
 
 Vector3 RayCast::get_cast_to() const {
@@ -94,6 +97,13 @@ void RayCast::set_enabled(bool p_enabled) {
 		set_fixed_process(p_enabled);
 	if (!p_enabled)
 		collided = false;
+
+	if (is_inside_tree() && get_tree()->is_debugging_collisions_hint()) {
+		if (p_enabled)
+			_update_debug_shape();
+		else
+			_clear_debug_shape();
+	}
 }
 
 bool RayCast::is_enabled() const {
@@ -109,6 +119,9 @@ void RayCast::_notification(int p_what) {
 
 			if (enabled && !get_tree()->is_editor_hint()) {
 				set_fixed_process(true);
+
+				if (get_tree()->is_debugging_collisions_hint())
+					_update_debug_shape();
 			} else
 				set_fixed_process(false);
 
@@ -119,13 +132,23 @@ void RayCast::_notification(int p_what) {
 				set_fixed_process(false);
 			}
 
+			if (debug_shape)
+				_clear_debug_shape();
+
 		} break;
 		case NOTIFICATION_FIXED_PROCESS: {
 
 			if (!enabled)
 				break;
 
+			bool prev_collision_state = collided;
 			_update_raycast_state();
+			if (prev_collision_state != collided && get_tree()->is_debugging_collisions_hint()) {
+				if (debug_material.is_valid()) {
+					Ref<FixedSpatialMaterial> line_material = static_cast<Ref<FixedSpatialMaterial> >(debug_material);
+					line_material->set_albedo(collided ? Color(1.0, 0, 0) : Color(1.0, 0.8, 0.6));
+				}
+			}
 
 		} break;
 	}
@@ -231,6 +254,68 @@ void RayCast::_bind_methods() {
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "type_mask", PROPERTY_HINT_FLAGS, "Static,Kinematic,Rigid,Character,Area"), "set_type_mask", "get_type_mask");
 }
 
+void RayCast::_create_debug_shape() {
+
+	if (!debug_material.is_valid()) {
+		debug_material = Ref<FixedSpatialMaterial>(memnew(FixedSpatialMaterial));
+
+		Ref<FixedSpatialMaterial> line_material = static_cast<Ref<FixedSpatialMaterial> >(debug_material);
+		line_material->set_flag(FixedSpatialMaterial::FLAG_UNSHADED, true);
+		line_material->set_line_width(3.0);
+		line_material->set_albedo(Color(1.0, 0.8, 0.6));
+	}
+
+	Ref<Mesh> mesh = memnew(Mesh);
+
+	MeshInstance *mi = memnew(MeshInstance);
+	mi->set_mesh(mesh);
+
+	add_child(mi);
+	debug_shape = mi;
+}
+
+void RayCast::_update_debug_shape() {
+
+	if (!enabled)
+		return;
+
+	if (!debug_shape)
+		_create_debug_shape();
+
+	MeshInstance *mi = static_cast<MeshInstance *>(debug_shape);
+	if (!mi->get_mesh().is_valid())
+		return;
+
+	Ref<Mesh> mesh = mi->get_mesh();
+	if (mesh->get_surface_count() > 0)
+		mesh->surface_remove(0);
+
+	Array a;
+	a.resize(Mesh::ARRAY_MAX);
+
+	Vector<Vector3> verts;
+	verts.push_back(Vector3());
+	verts.push_back(cast_to);
+	a[Mesh::ARRAY_VERTEX] = verts;
+
+	mesh->add_surface_from_arrays(Mesh::PRIMITIVE_LINES, a);
+	mesh->surface_set_material(0, debug_material);
+}
+
+void RayCast::_clear_debug_shape() {
+
+	if (!debug_shape)
+		return;
+
+	MeshInstance *mi = static_cast<MeshInstance *>(debug_shape);
+	if (mi->is_inside_tree())
+		mi->queue_delete();
+	else
+		memdelete(mi);
+
+	debug_shape = NULL;
+}
+
 RayCast::RayCast() {
 
 	enabled = false;
@@ -240,4 +325,5 @@ RayCast::RayCast() {
 	layer_mask = 1;
 	type_mask = PhysicsDirectSpaceState::TYPE_MASK_COLLISION;
 	cast_to = Vector3(0, -1, 0);
+	debug_shape = NULL;
 }
diff --git a/scene/3d/ray_cast.h b/scene/3d/ray_cast.h
index 00a10446a59..d1a284aee4d 100644
--- a/scene/3d/ray_cast.h
+++ b/scene/3d/ray_cast.h
@@ -49,6 +49,13 @@ class RayCast : public Spatial {
 	uint32_t layer_mask;
 	uint32_t type_mask;
 
+	Node *debug_shape;
+	Ref<Material> debug_material;
+
+	void _create_debug_shape();
+	void _update_debug_shape();
+	void _clear_debug_shape();
+
 protected:
 	void _notification(int p_what);
 	void _update_raycast_state();