initial commit, 4.5 stable

editor/scene/3d/gizmos/physics/SCsub

@@ -0,0 +1,6 @@
+#!/usr/bin/env python
+from misc.utility.scons_hints import *
+
+Import("env")
+
+env.add_source_files(env.editor_sources, "*.cpp")

editor/scene/3d/gizmos/physics/collision_object_3d_gizmo_plugin.cpp

@@ -0,0 +1,84 @@
+/**************************************************************************/
+/*  collision_object_3d_gizmo_plugin.cpp                                  */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "collision_object_3d_gizmo_plugin.h"
+
+#include "scene/3d/physics/collision_object_3d.h"
+#include "scene/3d/physics/collision_polygon_3d.h"
+#include "scene/3d/physics/collision_shape_3d.h"
+#include "scene/resources/surface_tool.h"
+
+CollisionObject3DGizmoPlugin::CollisionObject3DGizmoPlugin() {
+	const Color gizmo_color = SceneTree::get_singleton()->get_debug_collisions_color();
+	create_material("shape_material", gizmo_color);
+	const float gizmo_value = gizmo_color.get_v();
+	const Color gizmo_color_disabled = Color(gizmo_value, gizmo_value, gizmo_value, 0.65);
+	create_material("shape_material_disabled", gizmo_color_disabled);
+}
+
+bool CollisionObject3DGizmoPlugin::has_gizmo(Node3D *p_spatial) {
+	return Object::cast_to<CollisionObject3D>(p_spatial) != nullptr;
+}
+
+String CollisionObject3DGizmoPlugin::get_gizmo_name() const {
+	return "CollisionObject3D";
+}
+
+int CollisionObject3DGizmoPlugin::get_priority() const {
+	return -2;
+}
+
+void CollisionObject3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) {
+	CollisionObject3D *co = Object::cast_to<CollisionObject3D>(p_gizmo->get_node_3d());
+
+	p_gizmo->clear();
+
+	List<uint32_t> owner_ids;
+	co->get_shape_owners(&owner_ids);
+	for (uint32_t &owner_id : owner_ids) {
+		Transform3D xform = co->shape_owner_get_transform(owner_id);
+		Object *owner = co->shape_owner_get_owner(owner_id);
+		// Exclude CollisionShape3D and CollisionPolygon3D as they have their gizmo.
+		if (!Object::cast_to<CollisionShape3D>(owner) && !Object::cast_to<CollisionPolygon3D>(owner)) {
+			Ref<Material> material = get_material(!co->is_shape_owner_disabled(owner_id) ? "shape_material" : "shape_material_disabled", p_gizmo);
+			for (int shape_id = 0; shape_id < co->shape_owner_get_shape_count(owner_id); shape_id++) {
+				Ref<Shape3D> s = co->shape_owner_get_shape(owner_id, shape_id);
+				if (s.is_null()) {
+					continue;
+				}
+				SurfaceTool st;
+				st.append_from(s->get_debug_mesh(), 0, xform);
+
+				p_gizmo->add_mesh(st.commit(), material);
+				p_gizmo->add_collision_segments(s->get_debug_mesh_lines());
+			}
+		}
+	}
+}

editor/scene/3d/gizmos/physics/collision_object_3d_gizmo_plugin.h

@@ -0,0 +1,45 @@
+/**************************************************************************/
+/*  collision_object_3d_gizmo_plugin.h                                    */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "editor/scene/3d/node_3d_editor_gizmos.h"
+
+class CollisionObject3DGizmoPlugin : public EditorNode3DGizmoPlugin {
+	GDCLASS(CollisionObject3DGizmoPlugin, EditorNode3DGizmoPlugin);
+
+public:
+	bool has_gizmo(Node3D *p_spatial) override;
+	String get_gizmo_name() const override;
+	int get_priority() const override;
+	void redraw(EditorNode3DGizmo *p_gizmo) override;
+
+	CollisionObject3DGizmoPlugin();
+};

editor/scene/3d/gizmos/physics/collision_polygon_3d_gizmo_plugin.cpp

@@ -0,0 +1,237 @@
+/**************************************************************************/
+/*  collision_polygon_3d_gizmo_plugin.cpp                                 */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "collision_polygon_3d_gizmo_plugin.h"
+
+#include "core/math/geometry_2d.h"
+#include "scene/3d/physics/collision_polygon_3d.h"
+
+CollisionPolygon3DGizmoPlugin::CollisionPolygon3DGizmoPlugin() {
+	create_collision_material("shape_material", 2.0);
+	create_collision_material("shape_material_arraymesh", 0.0625);
+
+	create_collision_material("shape_material_disabled", 0.0625);
+	create_collision_material("shape_material_arraymesh_disabled", 0.015625);
+}
+
+void CollisionPolygon3DGizmoPlugin::create_collision_material(const String &p_name, float p_alpha) {
+	Vector<Ref<StandardMaterial3D>> mats;
+
+	const Color collision_color(1.0, 1.0, 1.0, p_alpha);
+
+	for (int i = 0; i < 4; i++) {
+		bool instantiated = i < 2;
+
+		Ref<StandardMaterial3D> material;
+		material.instantiate();
+
+		Color color = collision_color;
+		color.a *= instantiated ? 0.25 : 1.0;
+
+		material->set_albedo(color);
+		material->set_shading_mode(StandardMaterial3D::SHADING_MODE_UNSHADED);
+		material->set_transparency(StandardMaterial3D::TRANSPARENCY_ALPHA);
+		material->set_render_priority(StandardMaterial3D::RENDER_PRIORITY_MIN + 1);
+		material->set_cull_mode(StandardMaterial3D::CULL_BACK);
+		material->set_flag(StandardMaterial3D::FLAG_DISABLE_FOG, true);
+		material->set_flag(StandardMaterial3D::FLAG_ALBEDO_FROM_VERTEX_COLOR, true);
+		material->set_flag(StandardMaterial3D::FLAG_SRGB_VERTEX_COLOR, true);
+
+		mats.push_back(material);
+	}
+
+	materials[p_name] = mats;
+}
+
+bool CollisionPolygon3DGizmoPlugin::has_gizmo(Node3D *p_spatial) {
+	return Object::cast_to<CollisionPolygon3D>(p_spatial) != nullptr;
+}
+
+String CollisionPolygon3DGizmoPlugin::get_gizmo_name() const {
+	return "CollisionPolygon3D";
+}
+
+int CollisionPolygon3DGizmoPlugin::get_priority() const {
+	return -1;
+}
+
+void CollisionPolygon3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) {
+	CollisionPolygon3D *polygon = Object::cast_to<CollisionPolygon3D>(p_gizmo->get_node_3d());
+
+	p_gizmo->clear();
+
+	const Ref<StandardMaterial3D> material =
+			get_material(!polygon->is_disabled() ? "shape_material" : "shape_material_disabled", p_gizmo);
+	const Ref<StandardMaterial3D> material_arraymesh =
+			get_material(!polygon->is_disabled() ? "shape_material_arraymesh" : "shape_material_arraymesh_disabled", p_gizmo);
+
+	const Color collision_color = polygon->is_disabled() ? Color(1.0, 1.0, 1.0, 0.75) : polygon->get_debug_color();
+
+	Vector<Vector2> points = polygon->get_polygon();
+	float depth = polygon->get_depth() * 0.5;
+
+	Vector<Vector3> lines;
+	const int points_size = points.size();
+
+	for (int i = 0; i < points_size; i++) {
+		int n = (i + 1) % points_size;
+		lines.push_back(Vector3(points[i].x, points[i].y, depth));
+		lines.push_back(Vector3(points[n].x, points[n].y, depth));
+		lines.push_back(Vector3(points[i].x, points[i].y, -depth));
+		lines.push_back(Vector3(points[n].x, points[n].y, -depth));
+		lines.push_back(Vector3(points[i].x, points[i].y, depth));
+		lines.push_back(Vector3(points[i].x, points[i].y, -depth));
+	}
+
+	if (polygon->get_debug_fill_enabled()) {
+		Ref<ArrayMesh> array_mesh;
+		array_mesh.instantiate();
+
+		Vector<Vector3> verts;
+		Vector<Color> colors;
+		Vector<int> indices;
+
+		// Determine orientation of the 2D polygon's vertices to determine
+		// which direction to draw outer polygons.
+		float signed_area = 0.0f;
+		for (int i = 0; i < points_size; i++) {
+			const int j = (i + 1) % points_size;
+			signed_area += points[i].x * points[j].y - points[j].x * points[i].y;
+		}
+
+		// Generate triangles for the sides of the extruded polygon.
+		for (int i = 0; i < points_size; i++) {
+			verts.push_back(Vector3(points[i].x, points[i].y, depth));
+			verts.push_back(Vector3(points[i].x, points[i].y, -depth));
+
+			colors.push_back(collision_color);
+			colors.push_back(collision_color);
+		}
+
+		const int verts_size = verts.size();
+		for (int i = 0; i < verts_size; i += 2) {
+			const int j = (i + 1) % verts_size;
+			const int k = (i + 2) % verts_size;
+			const int l = (i + 3) % verts_size;
+
+			indices.push_back(i);
+			if (signed_area < 0) {
+				indices.push_back(j);
+				indices.push_back(k);
+			} else {
+				indices.push_back(k);
+				indices.push_back(j);
+			}
+
+			indices.push_back(j);
+			if (signed_area < 0) {
+				indices.push_back(l);
+				indices.push_back(k);
+			} else {
+				indices.push_back(k);
+				indices.push_back(l);
+			}
+		}
+
+		Vector<Vector<Vector2>> decomp = Geometry2D::decompose_polygon_in_convex(polygon->get_polygon());
+
+		// Generate triangles for the bottom cap of the extruded polygon.
+		for (int i = 0; i < decomp.size(); i++) {
+			Vector<Vector3> cap_verts_bottom;
+			Vector<Color> cap_colours_bottom;
+			Vector<int> cap_indices_bottom;
+
+			const int index_offset = verts_size;
+
+			const Vector<Vector2> &convex = decomp[i];
+			const int convex_size = convex.size();
+
+			for (int j = 0; j < convex_size; j++) {
+				cap_verts_bottom.push_back(Vector3(convex[j].x, convex[j].y, -depth));
+				cap_colours_bottom.push_back(collision_color);
+			}
+
+			if (convex_size >= 3) {
+				for (int j = 1; j < convex_size; j++) {
+					const int k = (j + 1) % convex_size;
+
+					cap_indices_bottom.push_back(index_offset + 0);
+					cap_indices_bottom.push_back(index_offset + j);
+					cap_indices_bottom.push_back(index_offset + k);
+				}
+			}
+			verts.append_array(cap_verts_bottom);
+			colors.append_array(cap_colours_bottom);
+			indices.append_array(cap_indices_bottom);
+		}
+
+		// Generate triangles for the top cap of the extruded polygon.
+		for (int i = 0; i < decomp.size(); i++) {
+			Vector<Vector3> cap_verts_top;
+			Vector<Color> cap_colours_top;
+			Vector<int> cap_indices_top;
+
+			const int index_offset = verts_size;
+
+			const Vector<Vector2> &convex = decomp[i];
+			const int convex_size = convex.size();
+
+			for (int j = 0; j < convex_size; j++) {
+				cap_verts_top.push_back(Vector3(convex[j].x, convex[j].y, depth));
+				cap_colours_top.push_back(collision_color);
+			}
+
+			if (convex_size >= 3) {
+				for (int j = 1; j < convex_size; j++) {
+					const int k = (j + 1) % convex_size;
+
+					cap_indices_top.push_back(index_offset + k);
+					cap_indices_top.push_back(index_offset + j);
+					cap_indices_top.push_back(index_offset + 0);
+				}
+			}
+			verts.append_array(cap_verts_top);
+			colors.append_array(cap_colours_top);
+			indices.append_array(cap_indices_top);
+		}
+
+		Array a;
+		a.resize(Mesh::ARRAY_MAX);
+		a[RS::ARRAY_VERTEX] = verts;
+		a[RS::ARRAY_COLOR] = colors;
+		a[RS::ARRAY_INDEX] = indices;
+		array_mesh->add_surface_from_arrays(Mesh::PRIMITIVE_TRIANGLES, a);
+
+		p_gizmo->add_mesh(array_mesh, material_arraymesh);
+	}
+
+	p_gizmo->add_lines(lines, material, false, collision_color);
+	p_gizmo->add_collision_segments(lines);
+}

editor/scene/3d/gizmos/physics/collision_polygon_3d_gizmo_plugin.h

@@ -0,0 +1,46 @@
+/**************************************************************************/
+/*  collision_polygon_3d_gizmo_plugin.h                                   */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "editor/scene/3d/node_3d_editor_gizmos.h"
+
+class CollisionPolygon3DGizmoPlugin : public EditorNode3DGizmoPlugin {
+	GDCLASS(CollisionPolygon3DGizmoPlugin, EditorNode3DGizmoPlugin);
+
+	void create_collision_material(const String &p_name, float p_alpha);
+
+public:
+	bool has_gizmo(Node3D *p_spatial) override;
+	String get_gizmo_name() const override;
+	int get_priority() const override;
+	void redraw(EditorNode3DGizmo *p_gizmo) override;
+	CollisionPolygon3DGizmoPlugin();
+};

editor/scene/3d/gizmos/physics/collision_shape_3d_gizmo_plugin.cpp

@@ -0,0 +1,669 @@
+/**************************************************************************/
+/*  collision_shape_3d_gizmo_plugin.cpp                                   */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "collision_shape_3d_gizmo_plugin.h"
+
+#include "core/math/convex_hull.h"
+#include "core/math/geometry_3d.h"
+#include "editor/editor_undo_redo_manager.h"
+#include "editor/scene/3d/gizmos/gizmo_3d_helper.h"
+#include "editor/scene/3d/node_3d_editor_plugin.h"
+#include "scene/3d/physics/collision_shape_3d.h"
+#include "scene/resources/3d/box_shape_3d.h"
+#include "scene/resources/3d/capsule_shape_3d.h"
+#include "scene/resources/3d/concave_polygon_shape_3d.h"
+#include "scene/resources/3d/convex_polygon_shape_3d.h"
+#include "scene/resources/3d/cylinder_shape_3d.h"
+#include "scene/resources/3d/height_map_shape_3d.h"
+#include "scene/resources/3d/separation_ray_shape_3d.h"
+#include "scene/resources/3d/sphere_shape_3d.h"
+#include "scene/resources/3d/world_boundary_shape_3d.h"
+
+CollisionShape3DGizmoPlugin::CollisionShape3DGizmoPlugin() {
+	helper.instantiate();
+
+	create_collision_material("shape_material", 2.0);
+	create_collision_material("shape_material_arraymesh", 0.0625);
+
+	create_collision_material("shape_material_disabled", 0.0625);
+	create_collision_material("shape_material_arraymesh_disabled", 0.015625);
+
+	create_handle_material("handles");
+}
+
+void CollisionShape3DGizmoPlugin::create_collision_material(const String &p_name, float p_alpha) {
+	Vector<Ref<StandardMaterial3D>> mats;
+
+	const Color collision_color(1.0, 1.0, 1.0, p_alpha);
+
+	for (int i = 0; i < 4; i++) {
+		bool instantiated = i < 2;
+
+		Ref<StandardMaterial3D> material = memnew(StandardMaterial3D);
+
+		Color color = collision_color;
+		color.a *= instantiated ? 0.25 : 1.0;
+
+		material->set_albedo(color);
+		material->set_shading_mode(StandardMaterial3D::SHADING_MODE_UNSHADED);
+		material->set_transparency(StandardMaterial3D::TRANSPARENCY_ALPHA);
+		material->set_render_priority(StandardMaterial3D::RENDER_PRIORITY_MIN + 1);
+		material->set_cull_mode(StandardMaterial3D::CULL_BACK);
+		material->set_flag(StandardMaterial3D::FLAG_DISABLE_FOG, true);
+		material->set_flag(StandardMaterial3D::FLAG_ALBEDO_FROM_VERTEX_COLOR, true);
+		material->set_flag(StandardMaterial3D::FLAG_SRGB_VERTEX_COLOR, true);
+
+		mats.push_back(material);
+	}
+
+	materials[p_name] = mats;
+}
+
+bool CollisionShape3DGizmoPlugin::has_gizmo(Node3D *p_spatial) {
+	return Object::cast_to<CollisionShape3D>(p_spatial) != nullptr;
+}
+
+String CollisionShape3DGizmoPlugin::get_gizmo_name() const {
+	return "CollisionShape3D";
+}
+
+int CollisionShape3DGizmoPlugin::get_priority() const {
+	return -1;
+}
+
+String CollisionShape3DGizmoPlugin::get_handle_name(const EditorNode3DGizmo *p_gizmo, int p_id, bool p_secondary) const {
+	const CollisionShape3D *cs = Object::cast_to<CollisionShape3D>(p_gizmo->get_node_3d());
+
+	Ref<Shape3D> s = cs->get_shape();
+	if (s.is_null()) {
+		return "";
+	}
+
+	if (Object::cast_to<SphereShape3D>(*s)) {
+		return "Radius";
+	}
+
+	if (Object::cast_to<BoxShape3D>(*s)) {
+		return helper->box_get_handle_name(p_id);
+	}
+
+	if (Object::cast_to<CapsuleShape3D>(*s)) {
+		return helper->capsule_get_handle_name(p_id);
+	}
+
+	if (Object::cast_to<CylinderShape3D>(*s)) {
+		return helper->cylinder_get_handle_name(p_id);
+	}
+
+	if (Object::cast_to<SeparationRayShape3D>(*s)) {
+		return "Length";
+	}
+
+	return "";
+}
+
+Variant CollisionShape3DGizmoPlugin::get_handle_value(const EditorNode3DGizmo *p_gizmo, int p_id, bool p_secondary) const {
+	CollisionShape3D *cs = Object::cast_to<CollisionShape3D>(p_gizmo->get_node_3d());
+
+	Ref<Shape3D> s = cs->get_shape();
+	if (s.is_null()) {
+		return Variant();
+	}
+
+	if (Object::cast_to<SphereShape3D>(*s)) {
+		Ref<SphereShape3D> ss = s;
+		return ss->get_radius();
+	}
+
+	if (Object::cast_to<BoxShape3D>(*s)) {
+		Ref<BoxShape3D> bs = s;
+		return bs->get_size();
+	}
+
+	if (Object::cast_to<CapsuleShape3D>(*s)) {
+		Ref<CapsuleShape3D> cs2 = s;
+		return Vector2(cs2->get_radius(), cs2->get_height());
+	}
+
+	if (Object::cast_to<CylinderShape3D>(*s)) {
+		Ref<CylinderShape3D> cs2 = s;
+		return Vector2(cs2->get_radius(), cs2->get_height());
+	}
+
+	if (Object::cast_to<SeparationRayShape3D>(*s)) {
+		Ref<SeparationRayShape3D> cs2 = s;
+		return cs2->get_length();
+	}
+
+	return Variant();
+}
+
+void CollisionShape3DGizmoPlugin::begin_handle_action(const EditorNode3DGizmo *p_gizmo, int p_id, bool p_secondary) {
+	helper->initialize_handle_action(get_handle_value(p_gizmo, p_id, p_secondary), p_gizmo->get_node_3d()->get_global_transform());
+}
+
+void CollisionShape3DGizmoPlugin::set_handle(const EditorNode3DGizmo *p_gizmo, int p_id, bool p_secondary, Camera3D *p_camera, const Point2 &p_point) {
+	CollisionShape3D *cs = Object::cast_to<CollisionShape3D>(p_gizmo->get_node_3d());
+
+	Ref<Shape3D> s = cs->get_shape();
+	if (s.is_null()) {
+		return;
+	}
+
+	Vector3 sg[2];
+	helper->get_segment(p_camera, p_point, sg);
+
+	if (Object::cast_to<SphereShape3D>(*s)) {
+		Ref<SphereShape3D> ss = s;
+		Vector3 ra, rb;
+		Geometry3D::get_closest_points_between_segments(Vector3(), Vector3(4096, 0, 0), sg[0], sg[1], ra, rb);
+		float d = ra.x;
+		if (Node3DEditor::get_singleton()->is_snap_enabled()) {
+			d = Math::snapped(d, Node3DEditor::get_singleton()->get_translate_snap());
+		}
+
+		if (d < 0.001) {
+			d = 0.001;
+		}
+
+		ss->set_radius(d);
+	}
+
+	if (Object::cast_to<SeparationRayShape3D>(*s)) {
+		Ref<SeparationRayShape3D> rs = s;
+		Vector3 ra, rb;
+		Geometry3D::get_closest_points_between_segments(Vector3(), Vector3(0, 0, 4096), sg[0], sg[1], ra, rb);
+		float d = ra.z;
+		if (Node3DEditor::get_singleton()->is_snap_enabled()) {
+			d = Math::snapped(d, Node3DEditor::get_singleton()->get_translate_snap());
+		}
+
+		if (d < 0.001) {
+			d = 0.001;
+		}
+
+		rs->set_length(d);
+	}
+
+	if (Object::cast_to<BoxShape3D>(*s)) {
+		Ref<BoxShape3D> bs = s;
+		Vector3 size = bs->get_size();
+		Vector3 position;
+		helper->box_set_handle(sg, p_id, size, position);
+		bs->set_size(size);
+		cs->set_global_position(position);
+	}
+
+	if (Object::cast_to<CapsuleShape3D>(*s)) {
+		Ref<CapsuleShape3D> cs2 = s;
+
+		real_t height = cs2->get_height();
+		real_t radius = cs2->get_radius();
+		Vector3 position;
+		helper->capsule_set_handle(sg, p_id, height, radius, position);
+		cs2->set_height(height);
+		cs2->set_radius(radius);
+		cs->set_global_position(position);
+	}
+
+	if (Object::cast_to<CylinderShape3D>(*s)) {
+		Ref<CylinderShape3D> cs2 = s;
+
+		real_t height = cs2->get_height();
+		real_t radius = cs2->get_radius();
+		Vector3 position;
+		helper->cylinder_set_handle(sg, p_id, height, radius, position);
+		cs2->set_height(height);
+		cs2->set_radius(radius);
+		cs->set_global_position(position);
+	}
+}
+
+void CollisionShape3DGizmoPlugin::commit_handle(const EditorNode3DGizmo *p_gizmo, int p_id, bool p_secondary, const Variant &p_restore, bool p_cancel) {
+	CollisionShape3D *cs = Object::cast_to<CollisionShape3D>(p_gizmo->get_node_3d());
+
+	Ref<Shape3D> s = cs->get_shape();
+	if (s.is_null()) {
+		return;
+	}
+
+	if (Object::cast_to<SphereShape3D>(*s)) {
+		Ref<SphereShape3D> ss = s;
+		if (p_cancel) {
+			ss->set_radius(p_restore);
+			return;
+		}
+
+		EditorUndoRedoManager *ur = EditorUndoRedoManager::get_singleton();
+		ur->create_action(TTR("Change Sphere Shape Radius"));
+		ur->add_do_method(ss.ptr(), "set_radius", ss->get_radius());
+		ur->add_undo_method(ss.ptr(), "set_radius", p_restore);
+		ur->commit_action();
+	}
+
+	if (Object::cast_to<BoxShape3D>(*s)) {
+		helper->box_commit_handle(TTR("Change Box Shape Size"), p_cancel, cs, s.ptr());
+	}
+
+	if (Object::cast_to<CapsuleShape3D>(*s)) {
+		Ref<CapsuleShape3D> ss = s;
+		helper->cylinder_commit_handle(p_id, TTR("Change Capsule Shape Radius"), TTR("Change Capsule Shape Height"), p_cancel, cs, *ss, *ss);
+	}
+
+	if (Object::cast_to<CylinderShape3D>(*s)) {
+		Ref<CylinderShape3D> ss = s;
+		helper->cylinder_commit_handle(p_id, TTR("Change Cylinder Shape Radius"), TTR("Change Cylinder Shape Height"), p_cancel, cs, *ss, *ss);
+	}
+
+	if (Object::cast_to<SeparationRayShape3D>(*s)) {
+		Ref<SeparationRayShape3D> ss = s;
+		if (p_cancel) {
+			ss->set_length(p_restore);
+			return;
+		}
+
+		EditorUndoRedoManager *ur = EditorUndoRedoManager::get_singleton();
+		ur->create_action(TTR("Change Separation Ray Shape Length"));
+		ur->add_do_method(ss.ptr(), "set_length", ss->get_length());
+		ur->add_undo_method(ss.ptr(), "set_length", p_restore);
+		ur->commit_action();
+	}
+}
+
+void CollisionShape3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) {
+	CollisionShape3D *cs = Object::cast_to<CollisionShape3D>(p_gizmo->get_node_3d());
+
+	p_gizmo->clear();
+
+	Ref<Shape3D> s = cs->get_shape();
+	if (s.is_null()) {
+		return;
+	}
+
+	const Ref<StandardMaterial3D> material =
+			get_material(!cs->is_disabled() ? "shape_material" : "shape_material_disabled", p_gizmo);
+	const Ref<StandardMaterial3D> material_arraymesh =
+			get_material(!cs->is_disabled() ? "shape_material_arraymesh" : "shape_material_arraymesh_disabled", p_gizmo);
+	const Ref<Material> handles_material = get_material("handles");
+
+	const Color collision_color = cs->is_disabled() ? Color(1.0, 1.0, 1.0, 0.75) : cs->get_debug_color();
+
+	if (cs->get_debug_fill_enabled()) {
+		Ref<ArrayMesh> array_mesh = s->get_debug_arraymesh_faces(collision_color);
+		if (array_mesh.is_valid() && array_mesh->get_surface_count() > 0) {
+			p_gizmo->add_mesh(array_mesh, material_arraymesh);
+		}
+	}
+
+	if (Object::cast_to<SphereShape3D>(*s)) {
+		Ref<SphereShape3D> sp = s;
+		float radius = sp->get_radius();
+
+#define PUSH_QUARTER(from_x, from_y, to_x, to_y, y)      \
+	points_ptrw[index++] = Vector3(from_x, y, from_y);   \
+	points_ptrw[index++] = Vector3(to_x, y, to_y);       \
+	points_ptrw[index++] = Vector3(from_x, y, -from_y);  \
+	points_ptrw[index++] = Vector3(to_x, y, -to_y);      \
+	points_ptrw[index++] = Vector3(-from_x, y, from_y);  \
+	points_ptrw[index++] = Vector3(-to_x, y, to_y);      \
+	points_ptrw[index++] = Vector3(-from_x, y, -from_y); \
+	points_ptrw[index++] = Vector3(-to_x, y, -to_y);
+
+#define PUSH_QUARTER_XY(from_x, from_y, to_x, to_y, y)       \
+	points_ptrw[index++] = Vector3(from_x, -from_y - y, 0);  \
+	points_ptrw[index++] = Vector3(to_x, -to_y - y, 0);      \
+	points_ptrw[index++] = Vector3(from_x, from_y + y, 0);   \
+	points_ptrw[index++] = Vector3(to_x, to_y + y, 0);       \
+	points_ptrw[index++] = Vector3(-from_x, -from_y - y, 0); \
+	points_ptrw[index++] = Vector3(-to_x, -to_y - y, 0);     \
+	points_ptrw[index++] = Vector3(-from_x, from_y + y, 0);  \
+	points_ptrw[index++] = Vector3(-to_x, to_y + y, 0);
+
+#define PUSH_QUARTER_YZ(from_x, from_y, to_x, to_y, y)       \
+	points_ptrw[index++] = Vector3(0, -from_y - y, from_x);  \
+	points_ptrw[index++] = Vector3(0, -to_y - y, to_x);      \
+	points_ptrw[index++] = Vector3(0, from_y + y, from_x);   \
+	points_ptrw[index++] = Vector3(0, to_y + y, to_x);       \
+	points_ptrw[index++] = Vector3(0, -from_y - y, -from_x); \
+	points_ptrw[index++] = Vector3(0, -to_y - y, -to_x);     \
+	points_ptrw[index++] = Vector3(0, from_y + y, -from_x);  \
+	points_ptrw[index++] = Vector3(0, to_y + y, -to_x);
+
+		// Number of points in an octant. So there will be 8 * points_in_octant * 2 points in total for one circle.
+		// This Corresponds to the smoothness of the circle.
+		const uint32_t points_in_octant = 16;
+		const real_t inc = (Math::PI / (4 * points_in_octant));
+		const real_t radius_squared = radius * radius;
+		real_t r = 0;
+
+		Vector<Vector3> points;
+		uint32_t index = 0;
+		// 3 full circles.
+		points.resize(3 * 8 * points_in_octant * 2);
+		Vector3 *points_ptrw = points.ptrw();
+
+		float previous_x = radius;
+		float previous_y = 0.f;
+
+		for (uint32_t i = 0; i < points_in_octant; ++i) {
+			r += inc;
+			real_t x = Math::cos(r) * radius;
+			real_t y = Math::sqrt(radius_squared - (x * x));
+
+			PUSH_QUARTER(previous_x, previous_y, x, y, 0);
+			PUSH_QUARTER(previous_y, previous_x, y, x, 0);
+
+			PUSH_QUARTER_XY(previous_x, previous_y, x, y, 0);
+			PUSH_QUARTER_XY(previous_y, previous_x, y, x, 0);
+
+			PUSH_QUARTER_YZ(previous_x, previous_y, x, y, 0);
+			PUSH_QUARTER_YZ(previous_y, previous_x, y, x, 0)
+
+			previous_x = x;
+			previous_y = y;
+		}
+#undef PUSH_QUARTER
+#undef PUSH_QUARTER_XY
+#undef PUSH_QUARTER_YZ
+
+		p_gizmo->add_lines(points, material, false, collision_color);
+		p_gizmo->add_collision_segments(points);
+		Vector<Vector3> handles;
+		handles.push_back(Vector3(radius, 0, 0));
+		p_gizmo->add_handles(handles, handles_material);
+	}
+
+	if (Object::cast_to<BoxShape3D>(*s)) {
+		Ref<BoxShape3D> bs = s;
+		Vector<Vector3> lines;
+		AABB aabb;
+		aabb.position = -bs->get_size() / 2;
+		aabb.size = bs->get_size();
+
+		for (int i = 0; i < 12; i++) {
+			Vector3 a, b;
+			aabb.get_edge(i, a, b);
+			lines.push_back(a);
+			lines.push_back(b);
+		}
+
+		const Vector<Vector3> handles = helper->box_get_handles(bs->get_size());
+
+		p_gizmo->add_lines(lines, material, false, collision_color);
+		p_gizmo->add_collision_segments(lines);
+		p_gizmo->add_handles(handles, handles_material);
+	}
+
+	if (Object::cast_to<CapsuleShape3D>(*s)) {
+		Ref<CapsuleShape3D> cs2 = s;
+		float radius = cs2->get_radius();
+		float height = cs2->get_height();
+
+		// Number of points in an octant. So there will be 8 * points_in_octant points in total.
+		// This corresponds to the smoothness of the circle.
+		const uint32_t points_in_octant = 16;
+		const real_t octant_angle = Math::PI / 4;
+		const real_t inc = (Math::PI / (4 * points_in_octant));
+		const real_t radius_squared = radius * radius;
+		real_t r = 0;
+
+		Vector<Vector3> points;
+		// 4 vertical lines and 4 full circles.
+		points.resize(4 * 2 + 4 * 8 * points_in_octant * 2);
+		Vector3 *points_ptrw = points.ptrw();
+
+		uint32_t index = 0;
+		float y_value = height * 0.5 - radius;
+
+		// Vertical Lines.
+		points_ptrw[index++] = Vector3(0.f, y_value, radius);
+		points_ptrw[index++] = Vector3(0.f, -y_value, radius);
+		points_ptrw[index++] = Vector3(0.f, y_value, -radius);
+		points_ptrw[index++] = Vector3(0.f, -y_value, -radius);
+		points_ptrw[index++] = Vector3(radius, y_value, 0.f);
+		points_ptrw[index++] = Vector3(radius, -y_value, 0.f);
+		points_ptrw[index++] = Vector3(-radius, y_value, 0.f);
+		points_ptrw[index++] = Vector3(-radius, -y_value, 0.f);
+
+#define PUSH_QUARTER(from_x, from_y, to_x, to_y, y)      \
+	points_ptrw[index++] = Vector3(from_x, y, from_y);   \
+	points_ptrw[index++] = Vector3(to_x, y, to_y);       \
+	points_ptrw[index++] = Vector3(from_x, y, -from_y);  \
+	points_ptrw[index++] = Vector3(to_x, y, -to_y);      \
+	points_ptrw[index++] = Vector3(-from_x, y, from_y);  \
+	points_ptrw[index++] = Vector3(-to_x, y, to_y);      \
+	points_ptrw[index++] = Vector3(-from_x, y, -from_y); \
+	points_ptrw[index++] = Vector3(-to_x, y, -to_y);
+
+#define PUSH_QUARTER_XY(from_x, from_y, to_x, to_y, y)       \
+	points_ptrw[index++] = Vector3(from_x, -from_y - y, 0);  \
+	points_ptrw[index++] = Vector3(to_x, -to_y - y, 0);      \
+	points_ptrw[index++] = Vector3(from_x, from_y + y, 0);   \
+	points_ptrw[index++] = Vector3(to_x, to_y + y, 0);       \
+	points_ptrw[index++] = Vector3(-from_x, -from_y - y, 0); \
+	points_ptrw[index++] = Vector3(-to_x, -to_y - y, 0);     \
+	points_ptrw[index++] = Vector3(-from_x, from_y + y, 0);  \
+	points_ptrw[index++] = Vector3(-to_x, to_y + y, 0);
+
+#define PUSH_QUARTER_YZ(from_x, from_y, to_x, to_y, y)       \
+	points_ptrw[index++] = Vector3(0, -from_y - y, from_x);  \
+	points_ptrw[index++] = Vector3(0, -to_y - y, to_x);      \
+	points_ptrw[index++] = Vector3(0, from_y + y, from_x);   \
+	points_ptrw[index++] = Vector3(0, to_y + y, to_x);       \
+	points_ptrw[index++] = Vector3(0, -from_y - y, -from_x); \
+	points_ptrw[index++] = Vector3(0, -to_y - y, -to_x);     \
+	points_ptrw[index++] = Vector3(0, from_y + y, -from_x);  \
+	points_ptrw[index++] = Vector3(0, to_y + y, -to_x);
+
+		float previous_x = radius;
+		float previous_y = 0.f;
+
+		for (uint32_t i = 0; i < points_in_octant; ++i) {
+			r += inc;
+			real_t x = Math::cos((i == points_in_octant - 1) ? octant_angle : r) * radius;
+			real_t y = Math::sqrt(radius_squared - (x * x));
+
+			// High circle ring.
+			PUSH_QUARTER(previous_x, previous_y, x, y, y_value);
+			PUSH_QUARTER(previous_y, previous_x, y, x, y_value);
+
+			// Low circle ring.
+			PUSH_QUARTER(previous_x, previous_y, x, y, -y_value);
+			PUSH_QUARTER(previous_y, previous_x, y, x, -y_value);
+
+			// Up and Low circle in X-Y plane.
+			PUSH_QUARTER_XY(previous_x, previous_y, x, y, y_value);
+			PUSH_QUARTER_XY(previous_y, previous_x, y, x, y_value);
+
+			// Up and Low circle in Y-Z plane.
+			PUSH_QUARTER_YZ(previous_x, previous_y, x, y, y_value);
+			PUSH_QUARTER_YZ(previous_y, previous_x, y, x, y_value)
+
+			previous_x = x;
+			previous_y = y;
+		}
+
+#undef PUSH_QUARTER
+#undef PUSH_QUARTER_XY
+#undef PUSH_QUARTER_YZ
+
+		p_gizmo->add_lines(points, material, false, collision_color);
+		p_gizmo->add_collision_segments(points);
+
+		Vector<Vector3> handles = helper->capsule_get_handles(cs2->get_height(), cs2->get_radius());
+		p_gizmo->add_handles(handles, handles_material);
+	}
+
+	if (Object::cast_to<CylinderShape3D>(*s)) {
+		Ref<CylinderShape3D> cs2 = s;
+		float radius = cs2->get_radius();
+		float height = cs2->get_height();
+
+#define PUSH_QUARTER(from_x, from_y, to_x, to_y, y)      \
+	points_ptrw[index++] = Vector3(from_x, y, from_y);   \
+	points_ptrw[index++] = Vector3(to_x, y, to_y);       \
+	points_ptrw[index++] = Vector3(from_x, y, -from_y);  \
+	points_ptrw[index++] = Vector3(to_x, y, -to_y);      \
+	points_ptrw[index++] = Vector3(-from_x, y, from_y);  \
+	points_ptrw[index++] = Vector3(-to_x, y, to_y);      \
+	points_ptrw[index++] = Vector3(-from_x, y, -from_y); \
+	points_ptrw[index++] = Vector3(-to_x, y, -to_y);
+
+		// Number of points in an octant. So there will be 8 * points_in_octant * 2 points in total for one circle.
+		// This corresponds to the smoothness of the circle.
+		const uint32_t points_in_octant = 16;
+		const real_t inc = (Math::PI / (4 * points_in_octant));
+		const real_t radius_squared = radius * radius;
+		real_t r = 0;
+
+		Vector<Vector3> points;
+		uint32_t index = 0;
+		// 4 vertical lines and 2 full circles.
+		points.resize(4 * 2 + 2 * 8 * points_in_octant * 2);
+		Vector3 *points_ptrw = points.ptrw();
+		float y_value = height * 0.5;
+
+		// Vertical lines.
+		points_ptrw[index++] = Vector3(0.f, y_value, radius);
+		points_ptrw[index++] = Vector3(0.f, -y_value, radius);
+		points_ptrw[index++] = Vector3(0.f, y_value, -radius);
+		points_ptrw[index++] = Vector3(0.f, -y_value, -radius);
+		points_ptrw[index++] = Vector3(radius, y_value, 0.f);
+		points_ptrw[index++] = Vector3(radius, -y_value, 0.f);
+		points_ptrw[index++] = Vector3(-radius, y_value, 0.f);
+		points_ptrw[index++] = Vector3(-radius, -y_value, 0.f);
+
+		float previous_x = radius;
+		float previous_y = 0.f;
+
+		for (uint32_t i = 0; i < points_in_octant; ++i) {
+			r += inc;
+			real_t x = Math::cos(r) * radius;
+			real_t y = Math::sqrt(radius_squared - (x * x));
+
+			// High circle ring.
+			PUSH_QUARTER(previous_x, previous_y, x, y, y_value);
+			PUSH_QUARTER(previous_y, previous_x, y, x, y_value);
+
+			// Low circle ring.
+			PUSH_QUARTER(previous_x, previous_y, x, y, -y_value);
+			PUSH_QUARTER(previous_y, previous_x, y, x, -y_value);
+
+			previous_x = x;
+			previous_y = y;
+		}
+#undef PUSH_QUARTER
+
+		p_gizmo->add_lines(points, material, false, collision_color);
+		p_gizmo->add_collision_segments(points);
+
+		Vector<Vector3> handles = helper->cylinder_get_handles(cs2->get_height(), cs2->get_radius());
+		p_gizmo->add_handles(handles, handles_material);
+	}
+
+	if (Object::cast_to<WorldBoundaryShape3D>(*s)) {
+		Ref<WorldBoundaryShape3D> wbs = s;
+		const Plane &p = wbs->get_plane();
+
+		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,
+		};
+
+		Vector<Vector3> points = {
+			pface[0],
+			pface[1],
+			pface[1],
+			pface[2],
+			pface[2],
+			pface[3],
+			pface[3],
+			pface[0],
+			p.normal * p.d,
+			p.normal * p.d + p.normal * 3
+		};
+
+		p_gizmo->add_lines(points, material, false, collision_color);
+		p_gizmo->add_collision_segments(points);
+	}
+
+	if (Object::cast_to<ConvexPolygonShape3D>(*s)) {
+		Vector<Vector3> points = Object::cast_to<ConvexPolygonShape3D>(*s)->get_points();
+
+		if (points.size() > 1) { // Need at least 2 points for a line.
+			Vector<Vector3> varr = Variant(points);
+			Geometry3D::MeshData md;
+			Error err = ConvexHullComputer::convex_hull(varr, md);
+			if (err == OK) {
+				Vector<Vector3> lines;
+				lines.resize(md.edges.size() * 2);
+				for (uint32_t i = 0; i < md.edges.size(); i++) {
+					lines.write[i * 2 + 0] = md.vertices[md.edges[i].vertex_a];
+					lines.write[i * 2 + 1] = md.vertices[md.edges[i].vertex_b];
+				}
+				p_gizmo->add_lines(lines, material, false, collision_color);
+				p_gizmo->add_collision_segments(lines);
+			}
+		}
+	}
+
+	if (Object::cast_to<ConcavePolygonShape3D>(*s)) {
+		Ref<ConcavePolygonShape3D> cs2 = s;
+		Ref<ArrayMesh> mesh = cs2->get_debug_mesh();
+		p_gizmo->add_lines(cs2->get_debug_mesh_lines(), material, false, collision_color);
+		p_gizmo->add_collision_segments(cs2->get_debug_mesh_lines());
+	}
+
+	if (Object::cast_to<SeparationRayShape3D>(*s)) {
+		Ref<SeparationRayShape3D> rs = s;
+
+		Vector<Vector3> points = {
+			Vector3(),
+			Vector3(0, 0, rs->get_length())
+		};
+		p_gizmo->add_lines(points, material, false, collision_color);
+		p_gizmo->add_collision_segments(points);
+		Vector<Vector3> handles;
+		handles.push_back(Vector3(0, 0, rs->get_length()));
+		p_gizmo->add_handles(handles, handles_material);
+	}
+
+	if (Object::cast_to<HeightMapShape3D>(*s)) {
+		Ref<HeightMapShape3D> hms = s;
+
+		Vector<Vector3> lines = hms->get_debug_mesh_lines();
+		p_gizmo->add_lines(lines, material, false, collision_color);
+	}
+}

editor/scene/3d/gizmos/physics/collision_shape_3d_gizmo_plugin.h

@@ -0,0 +1,57 @@
+/**************************************************************************/
+/*  collision_shape_3d_gizmo_plugin.h                                     */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "editor/scene/3d/node_3d_editor_gizmos.h"
+
+class Gizmo3DHelper;
+
+class CollisionShape3DGizmoPlugin : public EditorNode3DGizmoPlugin {
+	GDCLASS(CollisionShape3DGizmoPlugin, EditorNode3DGizmoPlugin);
+
+	void create_collision_material(const String &p_name, float p_alpha);
+
+	Ref<Gizmo3DHelper> helper;
+
+public:
+	bool has_gizmo(Node3D *p_spatial) override;
+	String get_gizmo_name() const override;
+	int get_priority() const override;
+	void redraw(EditorNode3DGizmo *p_gizmo) override;
+
+	String get_handle_name(const EditorNode3DGizmo *p_gizmo, int p_id, bool p_secondary) const override;
+	Variant get_handle_value(const EditorNode3DGizmo *p_gizmo, int p_id, bool p_secondary) const override;
+	void begin_handle_action(const EditorNode3DGizmo *p_gizmo, int p_id, bool p_secondary) override;
+	void set_handle(const EditorNode3DGizmo *p_gizmo, int p_id, bool p_secondary, Camera3D *p_camera, const Point2 &p_point) override;
+	void commit_handle(const EditorNode3DGizmo *p_gizmo, int p_id, bool p_secondary, const Variant &p_restore, bool p_cancel = false) override;
+
+	CollisionShape3DGizmoPlugin();
+};

editor/scene/3d/gizmos/physics/joint_3d_gizmo_plugin.cpp

@@ -0,0 +1,723 @@
+/**************************************************************************/
+/*  joint_3d_gizmo_plugin.cpp                                             */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "joint_3d_gizmo_plugin.h"
+
+#include "editor/editor_node.h"
+#include "editor/settings/editor_settings.h"
+#include "scene/3d/physics/joints/cone_twist_joint_3d.h"
+#include "scene/3d/physics/joints/generic_6dof_joint_3d.h"
+#include "scene/3d/physics/joints/hinge_joint_3d.h"
+#include "scene/3d/physics/joints/pin_joint_3d.h"
+#include "scene/3d/physics/joints/slider_joint_3d.h"
+
+#define BODY_A_RADIUS 0.25
+#define BODY_B_RADIUS 0.27
+
+Basis JointGizmosDrawer::look_body(const Transform3D &p_joint_transform, const Transform3D &p_body_transform) {
+	const Vector3 &p_eye(p_joint_transform.origin);
+	const Vector3 &p_target(p_body_transform.origin);
+
+	Vector3 v_x, v_y, v_z;
+
+	// Look the body with X
+	v_x = p_target - p_eye;
+	v_x.normalize();
+
+	v_z = v_x.cross(Vector3(0, 1, 0));
+	v_z.normalize();
+
+	v_y = v_z.cross(v_x);
+	v_y.normalize();
+
+	Basis base;
+	base.set_columns(v_x, v_y, v_z);
+
+	// Absorb current joint transform
+	base = p_joint_transform.basis.inverse() * base;
+
+	return base;
+}
+
+Basis JointGizmosDrawer::look_body_toward(Vector3::Axis p_axis, const Transform3D &joint_transform, const Transform3D &body_transform) {
+	switch (p_axis) {
+		case Vector3::AXIS_X:
+			return look_body_toward_x(joint_transform, body_transform);
+		case Vector3::AXIS_Y:
+			return look_body_toward_y(joint_transform, body_transform);
+		case Vector3::AXIS_Z:
+			return look_body_toward_z(joint_transform, body_transform);
+		default:
+			return Basis();
+	}
+}
+
+Basis JointGizmosDrawer::look_body_toward_x(const Transform3D &p_joint_transform, const Transform3D &p_body_transform) {
+	const Vector3 &p_eye(p_joint_transform.origin);
+	const Vector3 &p_target(p_body_transform.origin);
+
+	const Vector3 p_front(p_joint_transform.basis.get_column(0));
+
+	Vector3 v_x, v_y, v_z;
+
+	// Look the body with X
+	v_x = p_target - p_eye;
+	v_x.normalize();
+
+	v_y = p_front.cross(v_x);
+	v_y.normalize();
+
+	v_z = v_y.cross(p_front);
+	v_z.normalize();
+
+	// Clamp X to FRONT axis
+	v_x = p_front;
+	v_x.normalize();
+
+	Basis base;
+	base.set_columns(v_x, v_y, v_z);
+
+	// Absorb current joint transform
+	base = p_joint_transform.basis.inverse() * base;
+
+	return base;
+}
+
+Basis JointGizmosDrawer::look_body_toward_y(const Transform3D &p_joint_transform, const Transform3D &p_body_transform) {
+	const Vector3 &p_eye(p_joint_transform.origin);
+	const Vector3 &p_target(p_body_transform.origin);
+
+	const Vector3 p_up(p_joint_transform.basis.get_column(1));
+
+	Vector3 v_x, v_y, v_z;
+
+	// Look the body with X
+	v_x = p_target - p_eye;
+	v_x.normalize();
+
+	v_z = v_x.cross(p_up);
+	v_z.normalize();
+
+	v_x = p_up.cross(v_z);
+	v_x.normalize();
+
+	// Clamp Y to UP axis
+	v_y = p_up;
+	v_y.normalize();
+
+	Basis base;
+	base.set_columns(v_x, v_y, v_z);
+
+	// Absorb current joint transform
+	base = p_joint_transform.basis.inverse() * base;
+
+	return base;
+}
+
+Basis JointGizmosDrawer::look_body_toward_z(const Transform3D &p_joint_transform, const Transform3D &p_body_transform) {
+	const Vector3 &p_eye(p_joint_transform.origin);
+	const Vector3 &p_target(p_body_transform.origin);
+
+	const Vector3 p_lateral(p_joint_transform.basis.get_column(2));
+
+	Vector3 v_x, v_y, v_z;
+
+	// Look the body with X
+	v_x = p_target - p_eye;
+	v_x.normalize();
+
+	v_z = p_lateral;
+	v_z.normalize();
+
+	v_y = v_z.cross(v_x);
+	v_y.normalize();
+
+	// Clamp X to Z axis
+	v_x = v_y.cross(v_z);
+	v_x.normalize();
+
+	Basis base;
+	base.set_columns(v_x, v_y, v_z);
+
+	// Absorb current joint transform
+	base = p_joint_transform.basis.inverse() * base;
+
+	return base;
+}
+
+void JointGizmosDrawer::draw_circle(Vector3::Axis p_axis, real_t p_radius, const Transform3D &p_offset, const Basis &p_base, real_t p_limit_lower, real_t p_limit_upper, Vector<Vector3> &r_points, bool p_inverse) {
+	if (p_limit_lower == p_limit_upper) {
+		r_points.push_back(p_offset.translated_local(Vector3()).origin);
+		r_points.push_back(p_offset.translated_local(p_base.xform(Vector3(0.5, 0, 0))).origin);
+
+	} else {
+		if (p_limit_lower > p_limit_upper) {
+			p_limit_lower = -Math::PI;
+			p_limit_upper = Math::PI;
+		}
+
+		const int points = 32;
+
+		for (int i = 0; i < points; i++) {
+			real_t s = p_limit_lower + i * (p_limit_upper - p_limit_lower) / points;
+			real_t n = p_limit_lower + (i + 1) * (p_limit_upper - p_limit_lower) / points;
+
+			Vector3 from;
+			Vector3 to;
+			switch (p_axis) {
+				case Vector3::AXIS_X:
+					if (p_inverse) {
+						from = p_base.xform(Vector3(0, Math::sin(s), Math::cos(s))) * p_radius;
+						to = p_base.xform(Vector3(0, Math::sin(n), Math::cos(n))) * p_radius;
+					} else {
+						from = p_base.xform(Vector3(0, -Math::sin(s), Math::cos(s))) * p_radius;
+						to = p_base.xform(Vector3(0, -Math::sin(n), Math::cos(n))) * p_radius;
+					}
+					break;
+				case Vector3::AXIS_Y:
+					if (p_inverse) {
+						from = p_base.xform(Vector3(Math::cos(s), 0, -Math::sin(s))) * p_radius;
+						to = p_base.xform(Vector3(Math::cos(n), 0, -Math::sin(n))) * p_radius;
+					} else {
+						from = p_base.xform(Vector3(Math::cos(s), 0, Math::sin(s))) * p_radius;
+						to = p_base.xform(Vector3(Math::cos(n), 0, Math::sin(n))) * p_radius;
+					}
+					break;
+				case Vector3::AXIS_Z:
+					from = p_base.xform(Vector3(Math::cos(s), Math::sin(s), 0)) * p_radius;
+					to = p_base.xform(Vector3(Math::cos(n), Math::sin(n), 0)) * p_radius;
+					break;
+			}
+
+			if (i == points - 1) {
+				r_points.push_back(p_offset.translated_local(to).origin);
+				r_points.push_back(p_offset.translated_local(Vector3()).origin);
+			}
+			if (i == 0) {
+				r_points.push_back(p_offset.translated_local(from).origin);
+				r_points.push_back(p_offset.translated_local(Vector3()).origin);
+			}
+
+			r_points.push_back(p_offset.translated_local(from).origin);
+			r_points.push_back(p_offset.translated_local(to).origin);
+		}
+
+		r_points.push_back(p_offset.translated_local(Vector3(0, p_radius * 1.5, 0)).origin);
+		r_points.push_back(p_offset.translated_local(Vector3()).origin);
+	}
+}
+
+void JointGizmosDrawer::draw_cone(const Transform3D &p_offset, const Basis &p_base, real_t p_swing, real_t p_twist, Vector<Vector3> &r_points) {
+	float r = 1.0;
+	float w = r * Math::sin(p_swing);
+	float d = r * Math::cos(p_swing);
+
+	//swing
+	for (int i = 0; i < 360; i += 10) {
+		float ra = Math::deg_to_rad((float)i);
+		float rb = Math::deg_to_rad((float)i + 10);
+		Point2 a = Vector2(Math::sin(ra), Math::cos(ra)) * w;
+		Point2 b = Vector2(Math::sin(rb), Math::cos(rb)) * w;
+
+		r_points.push_back(p_offset.translated_local(p_base.xform(Vector3(d, a.x, a.y))).origin);
+		r_points.push_back(p_offset.translated_local(p_base.xform(Vector3(d, b.x, b.y))).origin);
+
+		if (i % 90 == 0) {
+			r_points.push_back(p_offset.translated_local(p_base.xform(Vector3(d, a.x, a.y))).origin);
+			r_points.push_back(p_offset.translated_local(p_base.xform(Vector3())).origin);
+		}
+	}
+
+	r_points.push_back(p_offset.translated_local(p_base.xform(Vector3())).origin);
+	r_points.push_back(p_offset.translated_local(p_base.xform(Vector3(1, 0, 0))).origin);
+
+	/// Twist
+	float ts = Math::rad_to_deg(p_twist);
+	ts = MIN(ts, 720);
+
+	for (int i = 0; i < int(ts); i += 5) {
+		float ra = Math::deg_to_rad((float)i);
+		float rb = Math::deg_to_rad((float)i + 5);
+		float c = i / 720.0;
+		float cn = (i + 5) / 720.0;
+		Point2 a = Vector2(Math::sin(ra), Math::cos(ra)) * w * c;
+		Point2 b = Vector2(Math::sin(rb), Math::cos(rb)) * w * cn;
+
+		r_points.push_back(p_offset.translated_local(p_base.xform(Vector3(c, a.x, a.y))).origin);
+		r_points.push_back(p_offset.translated_local(p_base.xform(Vector3(cn, b.x, b.y))).origin);
+	}
+}
+
+////
+
+Joint3DGizmoPlugin::Joint3DGizmoPlugin() {
+	create_material("joint_material", EDITOR_GET("editors/3d_gizmos/gizmo_colors/joint"));
+	create_material("joint_body_a_material", EDITOR_GET("editors/3d_gizmos/gizmo_colors/joint_body_a"));
+	create_material("joint_body_b_material", EDITOR_GET("editors/3d_gizmos/gizmo_colors/joint_body_b"));
+
+	update_timer = memnew(Timer);
+	update_timer->set_name("JointGizmoUpdateTimer");
+	update_timer->set_wait_time(1.0 / 120.0);
+	update_timer->connect("timeout", callable_mp(this, &Joint3DGizmoPlugin::incremental_update_gizmos));
+	update_timer->set_autostart(true);
+	callable_mp((Node *)EditorNode::get_singleton(), &Node::add_child).call_deferred(update_timer, false, Node::INTERNAL_MODE_DISABLED);
+}
+
+void Joint3DGizmoPlugin::incremental_update_gizmos() {
+	if (!current_gizmos.is_empty()) {
+		HashSet<EditorNode3DGizmo *>::Iterator E = current_gizmos.find(last_drawn);
+		if (E) {
+			++E;
+		}
+		if (!E) {
+			E = current_gizmos.begin();
+		}
+		redraw(*E);
+		last_drawn = *E;
+	}
+}
+
+bool Joint3DGizmoPlugin::has_gizmo(Node3D *p_spatial) {
+	return Object::cast_to<Joint3D>(p_spatial) != nullptr;
+}
+
+String Joint3DGizmoPlugin::get_gizmo_name() const {
+	return "Joint3D";
+}
+
+int Joint3DGizmoPlugin::get_priority() const {
+	return -1;
+}
+
+void Joint3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) {
+	Joint3D *joint = Object::cast_to<Joint3D>(p_gizmo->get_node_3d());
+
+	p_gizmo->clear();
+
+	Node3D *node_body_a = nullptr;
+	if (!joint->get_node_a().is_empty()) {
+		node_body_a = Object::cast_to<Node3D>(joint->get_node(joint->get_node_a()));
+	}
+
+	Node3D *node_body_b = nullptr;
+	if (!joint->get_node_b().is_empty()) {
+		node_body_b = Object::cast_to<Node3D>(joint->get_node(joint->get_node_b()));
+	}
+
+	if (!node_body_a && !node_body_b) {
+		return;
+	}
+
+	Ref<Material> common_material = get_material("joint_material", p_gizmo);
+	Ref<Material> body_a_material = get_material("joint_body_a_material", p_gizmo);
+	Ref<Material> body_b_material = get_material("joint_body_b_material", p_gizmo);
+
+	Vector<Vector3> points;
+	Vector<Vector3> body_a_points;
+	Vector<Vector3> body_b_points;
+
+	if (Object::cast_to<PinJoint3D>(joint)) {
+		CreatePinJointGizmo(Transform3D(), points);
+		p_gizmo->add_collision_segments(points);
+		p_gizmo->add_lines(points, common_material);
+	}
+
+	HingeJoint3D *hinge = Object::cast_to<HingeJoint3D>(joint);
+	if (hinge) {
+		CreateHingeJointGizmo(
+				Transform3D(),
+				hinge->get_global_transform(),
+				node_body_a ? node_body_a->get_global_transform() : Transform3D(),
+				node_body_b ? node_body_b->get_global_transform() : Transform3D(),
+				hinge->get_param(HingeJoint3D::PARAM_LIMIT_LOWER),
+				hinge->get_param(HingeJoint3D::PARAM_LIMIT_UPPER),
+				hinge->get_flag(HingeJoint3D::FLAG_USE_LIMIT),
+				points,
+				node_body_a ? &body_a_points : nullptr,
+				node_body_b ? &body_b_points : nullptr);
+
+		p_gizmo->add_collision_segments(points);
+		p_gizmo->add_collision_segments(body_a_points);
+		p_gizmo->add_collision_segments(body_b_points);
+
+		p_gizmo->add_lines(points, common_material);
+		p_gizmo->add_lines(body_a_points, body_a_material);
+		p_gizmo->add_lines(body_b_points, body_b_material);
+	}
+
+	SliderJoint3D *slider = Object::cast_to<SliderJoint3D>(joint);
+	if (slider) {
+		CreateSliderJointGizmo(
+				Transform3D(),
+				slider->get_global_transform(),
+				node_body_a ? node_body_a->get_global_transform() : Transform3D(),
+				node_body_b ? node_body_b->get_global_transform() : Transform3D(),
+				slider->get_param(SliderJoint3D::PARAM_ANGULAR_LIMIT_LOWER),
+				slider->get_param(SliderJoint3D::PARAM_ANGULAR_LIMIT_UPPER),
+				slider->get_param(SliderJoint3D::PARAM_LINEAR_LIMIT_LOWER),
+				slider->get_param(SliderJoint3D::PARAM_LINEAR_LIMIT_UPPER),
+				points,
+				node_body_a ? &body_a_points : nullptr,
+				node_body_b ? &body_b_points : nullptr);
+
+		p_gizmo->add_collision_segments(points);
+		p_gizmo->add_collision_segments(body_a_points);
+		p_gizmo->add_collision_segments(body_b_points);
+
+		p_gizmo->add_lines(points, common_material);
+		p_gizmo->add_lines(body_a_points, body_a_material);
+		p_gizmo->add_lines(body_b_points, body_b_material);
+	}
+
+	ConeTwistJoint3D *cone = Object::cast_to<ConeTwistJoint3D>(joint);
+	if (cone) {
+		CreateConeTwistJointGizmo(
+				Transform3D(),
+				cone->get_global_transform(),
+				node_body_a ? node_body_a->get_global_transform() : Transform3D(),
+				node_body_b ? node_body_b->get_global_transform() : Transform3D(),
+				cone->get_param(ConeTwistJoint3D::PARAM_SWING_SPAN),
+				cone->get_param(ConeTwistJoint3D::PARAM_TWIST_SPAN),
+				node_body_a ? &body_a_points : nullptr,
+				node_body_b ? &body_b_points : nullptr);
+
+		p_gizmo->add_collision_segments(body_a_points);
+		p_gizmo->add_collision_segments(body_b_points);
+
+		p_gizmo->add_lines(body_a_points, body_a_material);
+		p_gizmo->add_lines(body_b_points, body_b_material);
+	}
+
+	Generic6DOFJoint3D *gen = Object::cast_to<Generic6DOFJoint3D>(joint);
+	if (gen) {
+		CreateGeneric6DOFJointGizmo(
+				Transform3D(),
+				gen->get_global_transform(),
+				node_body_a ? node_body_a->get_global_transform() : Transform3D(),
+				node_body_b ? node_body_b->get_global_transform() : Transform3D(),
+
+				gen->get_param_x(Generic6DOFJoint3D::PARAM_ANGULAR_LOWER_LIMIT),
+				gen->get_param_x(Generic6DOFJoint3D::PARAM_ANGULAR_UPPER_LIMIT),
+				gen->get_param_x(Generic6DOFJoint3D::PARAM_LINEAR_LOWER_LIMIT),
+				gen->get_param_x(Generic6DOFJoint3D::PARAM_LINEAR_UPPER_LIMIT),
+				gen->get_flag_x(Generic6DOFJoint3D::FLAG_ENABLE_ANGULAR_LIMIT),
+				gen->get_flag_x(Generic6DOFJoint3D::FLAG_ENABLE_LINEAR_LIMIT),
+
+				gen->get_param_y(Generic6DOFJoint3D::PARAM_ANGULAR_LOWER_LIMIT),
+				gen->get_param_y(Generic6DOFJoint3D::PARAM_ANGULAR_UPPER_LIMIT),
+				gen->get_param_y(Generic6DOFJoint3D::PARAM_LINEAR_LOWER_LIMIT),
+				gen->get_param_y(Generic6DOFJoint3D::PARAM_LINEAR_UPPER_LIMIT),
+				gen->get_flag_y(Generic6DOFJoint3D::FLAG_ENABLE_ANGULAR_LIMIT),
+				gen->get_flag_y(Generic6DOFJoint3D::FLAG_ENABLE_LINEAR_LIMIT),
+
+				gen->get_param_z(Generic6DOFJoint3D::PARAM_ANGULAR_LOWER_LIMIT),
+				gen->get_param_z(Generic6DOFJoint3D::PARAM_ANGULAR_UPPER_LIMIT),
+				gen->get_param_z(Generic6DOFJoint3D::PARAM_LINEAR_LOWER_LIMIT),
+				gen->get_param_z(Generic6DOFJoint3D::PARAM_LINEAR_UPPER_LIMIT),
+				gen->get_flag_z(Generic6DOFJoint3D::FLAG_ENABLE_ANGULAR_LIMIT),
+				gen->get_flag_z(Generic6DOFJoint3D::FLAG_ENABLE_LINEAR_LIMIT),
+
+				points,
+				node_body_a ? &body_a_points : nullptr,
+				node_body_a ? &body_b_points : nullptr);
+
+		p_gizmo->add_collision_segments(points);
+		p_gizmo->add_collision_segments(body_a_points);
+		p_gizmo->add_collision_segments(body_b_points);
+
+		p_gizmo->add_lines(points, common_material);
+		p_gizmo->add_lines(body_a_points, body_a_material);
+		p_gizmo->add_lines(body_b_points, body_b_material);
+	}
+}
+
+void Joint3DGizmoPlugin::CreatePinJointGizmo(const Transform3D &p_offset, Vector<Vector3> &r_cursor_points) {
+	float cs = 0.25;
+
+	r_cursor_points.push_back(p_offset.translated_local(Vector3(+cs, 0, 0)).origin);
+	r_cursor_points.push_back(p_offset.translated_local(Vector3(-cs, 0, 0)).origin);
+	r_cursor_points.push_back(p_offset.translated_local(Vector3(0, +cs, 0)).origin);
+	r_cursor_points.push_back(p_offset.translated_local(Vector3(0, -cs, 0)).origin);
+	r_cursor_points.push_back(p_offset.translated_local(Vector3(0, 0, +cs)).origin);
+	r_cursor_points.push_back(p_offset.translated_local(Vector3(0, 0, -cs)).origin);
+}
+
+void Joint3DGizmoPlugin::CreateHingeJointGizmo(const Transform3D &p_offset, const Transform3D &p_trs_joint, const Transform3D &p_trs_body_a, const Transform3D &p_trs_body_b, real_t p_limit_lower, real_t p_limit_upper, bool p_use_limit, Vector<Vector3> &r_common_points, Vector<Vector3> *r_body_a_points, Vector<Vector3> *r_body_b_points) {
+	r_common_points.push_back(p_offset.translated_local(Vector3(0, 0, 0.5)).origin);
+	r_common_points.push_back(p_offset.translated_local(Vector3(0, 0, -0.5)).origin);
+
+	if (!p_use_limit) {
+		p_limit_upper = -1;
+		p_limit_lower = 0;
+	}
+
+	if (r_body_a_points) {
+		JointGizmosDrawer::draw_circle(Vector3::AXIS_Z,
+				BODY_A_RADIUS,
+				p_offset,
+				JointGizmosDrawer::look_body_toward_z(p_trs_joint, p_trs_body_a),
+				p_limit_lower,
+				p_limit_upper,
+				*r_body_a_points);
+	}
+
+	if (r_body_b_points) {
+		JointGizmosDrawer::draw_circle(Vector3::AXIS_Z,
+				BODY_B_RADIUS,
+				p_offset,
+				JointGizmosDrawer::look_body_toward_z(p_trs_joint, p_trs_body_b),
+				p_limit_lower,
+				p_limit_upper,
+				*r_body_b_points);
+	}
+}
+
+void Joint3DGizmoPlugin::CreateSliderJointGizmo(const Transform3D &p_offset, const Transform3D &p_trs_joint, const Transform3D &p_trs_body_a, const Transform3D &p_trs_body_b, real_t p_angular_limit_lower, real_t p_angular_limit_upper, real_t p_linear_limit_lower, real_t p_linear_limit_upper, Vector<Vector3> &r_points, Vector<Vector3> *r_body_a_points, Vector<Vector3> *r_body_b_points) {
+	p_linear_limit_lower = -p_linear_limit_lower;
+	p_linear_limit_upper = -p_linear_limit_upper;
+
+	float cs = 0.25;
+	r_points.push_back(p_offset.translated_local(Vector3(0, 0, 0.5)).origin);
+	r_points.push_back(p_offset.translated_local(Vector3(0, 0, -0.5)).origin);
+
+	if (p_linear_limit_lower >= p_linear_limit_upper) {
+		r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_upper, 0, 0)).origin);
+		r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_lower, 0, 0)).origin);
+
+		r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_upper, -cs, -cs)).origin);
+		r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_upper, -cs, cs)).origin);
+		r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_upper, -cs, cs)).origin);
+		r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_upper, cs, cs)).origin);
+		r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_upper, cs, cs)).origin);
+		r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_upper, cs, -cs)).origin);
+		r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_upper, cs, -cs)).origin);
+		r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_upper, -cs, -cs)).origin);
+
+		r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_lower, -cs, -cs)).origin);
+		r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_lower, -cs, cs)).origin);
+		r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_lower, -cs, cs)).origin);
+		r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_lower, cs, cs)).origin);
+		r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_lower, cs, cs)).origin);
+		r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_lower, cs, -cs)).origin);
+		r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_lower, cs, -cs)).origin);
+		r_points.push_back(p_offset.translated_local(Vector3(p_linear_limit_lower, -cs, -cs)).origin);
+
+	} else {
+		r_points.push_back(p_offset.translated_local(Vector3(+cs * 2, 0, 0)).origin);
+		r_points.push_back(p_offset.translated_local(Vector3(-cs * 2, 0, 0)).origin);
+	}
+
+	if (r_body_a_points) {
+		JointGizmosDrawer::draw_circle(
+				Vector3::AXIS_X,
+				BODY_A_RADIUS,
+				p_offset,
+				JointGizmosDrawer::look_body_toward(Vector3::AXIS_X, p_trs_joint, p_trs_body_a),
+				p_angular_limit_lower,
+				p_angular_limit_upper,
+				*r_body_a_points);
+	}
+
+	if (r_body_b_points) {
+		JointGizmosDrawer::draw_circle(
+				Vector3::AXIS_X,
+				BODY_B_RADIUS,
+				p_offset,
+				JointGizmosDrawer::look_body_toward(Vector3::AXIS_X, p_trs_joint, p_trs_body_b),
+				p_angular_limit_lower,
+				p_angular_limit_upper,
+				*r_body_b_points,
+				true);
+	}
+}
+
+void Joint3DGizmoPlugin::CreateConeTwistJointGizmo(const Transform3D &p_offset, const Transform3D &p_trs_joint, const Transform3D &p_trs_body_a, const Transform3D &p_trs_body_b, real_t p_swing, real_t p_twist, Vector<Vector3> *r_body_a_points, Vector<Vector3> *r_body_b_points) {
+	if (r_body_a_points) {
+		JointGizmosDrawer::draw_cone(
+				p_offset,
+				JointGizmosDrawer::look_body(p_trs_joint, p_trs_body_a),
+				p_swing,
+				p_twist,
+				*r_body_a_points);
+	}
+
+	if (r_body_b_points) {
+		JointGizmosDrawer::draw_cone(
+				p_offset,
+				JointGizmosDrawer::look_body(p_trs_joint, p_trs_body_b),
+				p_swing,
+				p_twist,
+				*r_body_b_points);
+	}
+}
+
+void Joint3DGizmoPlugin::CreateGeneric6DOFJointGizmo(
+		const Transform3D &p_offset,
+		const Transform3D &p_trs_joint,
+		const Transform3D &p_trs_body_a,
+		const Transform3D &p_trs_body_b,
+		real_t p_angular_limit_lower_x,
+		real_t p_angular_limit_upper_x,
+		real_t p_linear_limit_lower_x,
+		real_t p_linear_limit_upper_x,
+		bool p_enable_angular_limit_x,
+		bool p_enable_linear_limit_x,
+		real_t p_angular_limit_lower_y,
+		real_t p_angular_limit_upper_y,
+		real_t p_linear_limit_lower_y,
+		real_t p_linear_limit_upper_y,
+		bool p_enable_angular_limit_y,
+		bool p_enable_linear_limit_y,
+		real_t p_angular_limit_lower_z,
+		real_t p_angular_limit_upper_z,
+		real_t p_linear_limit_lower_z,
+		real_t p_linear_limit_upper_z,
+		bool p_enable_angular_limit_z,
+		bool p_enable_linear_limit_z,
+		Vector<Vector3> &r_points,
+		Vector<Vector3> *r_body_a_points,
+		Vector<Vector3> *r_body_b_points) {
+	float cs = 0.25;
+
+	for (int ax = 0; ax < 3; ax++) {
+		float ll = 0;
+		float ul = 0;
+		float lll = 0;
+		float lul = 0;
+
+		int a1 = 0;
+		int a2 = 0;
+		int a3 = 0;
+		bool enable_ang = false;
+		bool enable_lin = false;
+
+		switch (ax) {
+			case 0:
+				ll = p_angular_limit_lower_x;
+				ul = p_angular_limit_upper_x;
+				lll = -p_linear_limit_lower_x;
+				lul = -p_linear_limit_upper_x;
+				enable_ang = p_enable_angular_limit_x;
+				enable_lin = p_enable_linear_limit_x;
+				a1 = 0;
+				a2 = 1;
+				a3 = 2;
+				break;
+			case 1:
+				ll = p_angular_limit_lower_y;
+				ul = p_angular_limit_upper_y;
+				lll = -p_linear_limit_lower_y;
+				lul = -p_linear_limit_upper_y;
+				enable_ang = p_enable_angular_limit_y;
+				enable_lin = p_enable_linear_limit_y;
+				a1 = 1;
+				a2 = 2;
+				a3 = 0;
+				break;
+			case 2:
+				ll = p_angular_limit_lower_z;
+				ul = p_angular_limit_upper_z;
+				lll = -p_linear_limit_lower_z;
+				lul = -p_linear_limit_upper_z;
+				enable_ang = p_enable_angular_limit_z;
+				enable_lin = p_enable_linear_limit_z;
+				a1 = 2;
+				a2 = 0;
+				a3 = 1;
+				break;
+		}
+
+#define ADD_VTX(x, y, z)                                         \
+	{                                                            \
+		Vector3 v;                                               \
+		v[a1] = (x);                                             \
+		v[a2] = (y);                                             \
+		v[a3] = (z);                                             \
+		r_points.push_back(p_offset.translated_local(v).origin); \
+	}
+
+		if (enable_lin && lll >= lul) {
+			ADD_VTX(lul, 0, 0);
+			ADD_VTX(lll, 0, 0);
+
+			ADD_VTX(lul, -cs, -cs);
+			ADD_VTX(lul, -cs, cs);
+			ADD_VTX(lul, -cs, cs);
+			ADD_VTX(lul, cs, cs);
+			ADD_VTX(lul, cs, cs);
+			ADD_VTX(lul, cs, -cs);
+			ADD_VTX(lul, cs, -cs);
+			ADD_VTX(lul, -cs, -cs);
+
+			ADD_VTX(lll, -cs, -cs);
+			ADD_VTX(lll, -cs, cs);
+			ADD_VTX(lll, -cs, cs);
+			ADD_VTX(lll, cs, cs);
+			ADD_VTX(lll, cs, cs);
+			ADD_VTX(lll, cs, -cs);
+			ADD_VTX(lll, cs, -cs);
+			ADD_VTX(lll, -cs, -cs);
+
+		} else {
+			ADD_VTX(+cs * 2, 0, 0);
+			ADD_VTX(-cs * 2, 0, 0);
+		}
+
+		if (!enable_ang) {
+			ll = 0;
+			ul = -1;
+		}
+
+		if (r_body_a_points) {
+			JointGizmosDrawer::draw_circle(
+					static_cast<Vector3::Axis>(ax),
+					BODY_A_RADIUS,
+					p_offset,
+					JointGizmosDrawer::look_body_toward(static_cast<Vector3::Axis>(ax), p_trs_joint, p_trs_body_a),
+					ll,
+					ul,
+					*r_body_a_points,
+					true);
+		}
+
+		if (r_body_b_points) {
+			JointGizmosDrawer::draw_circle(
+					static_cast<Vector3::Axis>(ax),
+					BODY_B_RADIUS,
+					p_offset,
+					JointGizmosDrawer::look_body_toward(static_cast<Vector3::Axis>(ax), p_trs_joint, p_trs_body_b),
+					ll,
+					ul,
+					*r_body_b_points);
+		}
+	}
+
+#undef ADD_VTX
+}

editor/scene/3d/gizmos/physics/joint_3d_gizmo_plugin.h

@@ -0,0 +1,96 @@
+/**************************************************************************/
+/*  joint_3d_gizmo_plugin.h                                               */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "editor/scene/3d/node_3d_editor_gizmos.h"
+
+class Joint3DGizmoPlugin : public EditorNode3DGizmoPlugin {
+	GDCLASS(Joint3DGizmoPlugin, EditorNode3DGizmoPlugin);
+
+	Timer *update_timer = nullptr;
+	EditorNode3DGizmo *last_drawn = nullptr;
+
+	void incremental_update_gizmos();
+
+public:
+	bool has_gizmo(Node3D *p_spatial) override;
+	String get_gizmo_name() const override;
+	int get_priority() const override;
+	void redraw(EditorNode3DGizmo *p_gizmo) override;
+
+	static void CreatePinJointGizmo(const Transform3D &p_offset, Vector<Vector3> &r_cursor_points);
+	static void CreateHingeJointGizmo(const Transform3D &p_offset, const Transform3D &p_trs_joint, const Transform3D &p_trs_body_a, const Transform3D &p_trs_body_b, real_t p_limit_lower, real_t p_limit_upper, bool p_use_limit, Vector<Vector3> &r_common_points, Vector<Vector3> *r_body_a_points, Vector<Vector3> *r_body_b_points);
+	static void CreateSliderJointGizmo(const Transform3D &p_offset, const Transform3D &p_trs_joint, const Transform3D &p_trs_body_a, const Transform3D &p_trs_body_b, real_t p_angular_limit_lower, real_t p_angular_limit_upper, real_t p_linear_limit_lower, real_t p_linear_limit_upper, Vector<Vector3> &r_points, Vector<Vector3> *r_body_a_points, Vector<Vector3> *r_body_b_points);
+	static void CreateConeTwistJointGizmo(const Transform3D &p_offset, const Transform3D &p_trs_joint, const Transform3D &p_trs_body_a, const Transform3D &p_trs_body_b, real_t p_swing, real_t p_twist, Vector<Vector3> *r_body_a_points, Vector<Vector3> *r_body_b_points);
+	static void CreateGeneric6DOFJointGizmo(
+			const Transform3D &p_offset,
+			const Transform3D &p_trs_joint,
+			const Transform3D &p_trs_body_a,
+			const Transform3D &p_trs_body_b,
+			real_t p_angular_limit_lower_x,
+			real_t p_angular_limit_upper_x,
+			real_t p_linear_limit_lower_x,
+			real_t p_linear_limit_upper_x,
+			bool p_enable_angular_limit_x,
+			bool p_enable_linear_limit_x,
+			real_t p_angular_limit_lower_y,
+			real_t p_angular_limit_upper_y,
+			real_t p_linear_limit_lower_y,
+			real_t p_linear_limit_upper_y,
+			bool p_enable_angular_limit_y,
+			bool p_enable_linear_limit_y,
+			real_t p_angular_limit_lower_z,
+			real_t p_angular_limit_upper_z,
+			real_t p_linear_limit_lower_z,
+			real_t p_linear_limit_upper_z,
+			bool p_enable_angular_limit_z,
+			bool p_enable_linear_limit_z,
+			Vector<Vector3> &r_points,
+			Vector<Vector3> *r_body_a_points,
+			Vector<Vector3> *r_body_b_points);
+
+	Joint3DGizmoPlugin();
+};
+
+class JointGizmosDrawer {
+public:
+	static Basis look_body(const Transform3D &p_joint_transform, const Transform3D &p_body_transform);
+	static Basis look_body_toward(Vector3::Axis p_axis, const Transform3D &joint_transform, const Transform3D &body_transform);
+	static Basis look_body_toward_x(const Transform3D &p_joint_transform, const Transform3D &p_body_transform);
+	static Basis look_body_toward_y(const Transform3D &p_joint_transform, const Transform3D &p_body_transform);
+	/// Special function just used for physics joints, it returns a basis constrained toward Joint Z axis
+	/// with axis X and Y that are looking toward the body and oriented toward up
+	static Basis look_body_toward_z(const Transform3D &p_joint_transform, const Transform3D &p_body_transform);
+
+	// Draw circle around p_axis
+	static void draw_circle(Vector3::Axis p_axis, real_t p_radius, const Transform3D &p_offset, const Basis &p_base, real_t p_limit_lower, real_t p_limit_upper, Vector<Vector3> &r_points, bool p_inverse = false);
+	static void draw_cone(const Transform3D &p_offset, const Basis &p_base, real_t p_swing, real_t p_twist, Vector<Vector3> &r_points);
+};

editor/scene/3d/gizmos/physics/physics_bone_3d_gizmo_plugin.cpp

@@ -0,0 +1,167 @@
+/**************************************************************************/
+/*  physics_bone_3d_gizmo_plugin.cpp                                      */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "physics_bone_3d_gizmo_plugin.h"
+
+#include "editor/scene/3d/gizmos/physics/joint_3d_gizmo_plugin.h"
+#include "editor/settings/editor_settings.h"
+#include "scene/3d/physics/physical_bone_3d.h"
+#include "scene/3d/physics/physical_bone_simulator_3d.h"
+
+PhysicalBone3DGizmoPlugin::PhysicalBone3DGizmoPlugin() {
+	create_material("joint_material", EDITOR_GET("editors/3d_gizmos/gizmo_colors/joint"));
+}
+
+bool PhysicalBone3DGizmoPlugin::has_gizmo(Node3D *p_spatial) {
+	return Object::cast_to<PhysicalBone3D>(p_spatial) != nullptr;
+}
+
+String PhysicalBone3DGizmoPlugin::get_gizmo_name() const {
+	return "PhysicalBone3D";
+}
+
+int PhysicalBone3DGizmoPlugin::get_priority() const {
+	return -1;
+}
+
+void PhysicalBone3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) {
+	p_gizmo->clear();
+
+	PhysicalBone3D *physical_bone = Object::cast_to<PhysicalBone3D>(p_gizmo->get_node_3d());
+
+	if (!physical_bone) {
+		return;
+	}
+
+	PhysicalBoneSimulator3D *sm(physical_bone->get_simulator());
+	if (!sm) {
+		return;
+	}
+
+	PhysicalBone3D *pb(sm->get_physical_bone(physical_bone->get_bone_id()));
+	if (!pb) {
+		return;
+	}
+
+	PhysicalBone3D *pbp(sm->get_physical_bone_parent(physical_bone->get_bone_id()));
+	if (!pbp) {
+		return;
+	}
+
+	Vector<Vector3> points;
+
+	switch (physical_bone->get_joint_type()) {
+		case PhysicalBone3D::JOINT_TYPE_PIN: {
+			Joint3DGizmoPlugin::CreatePinJointGizmo(physical_bone->get_joint_offset(), points);
+		} break;
+		case PhysicalBone3D::JOINT_TYPE_CONE: {
+			const PhysicalBone3D::ConeJointData *cjd(static_cast<const PhysicalBone3D::ConeJointData *>(physical_bone->get_joint_data()));
+			Joint3DGizmoPlugin::CreateConeTwistJointGizmo(
+					physical_bone->get_joint_offset(),
+					physical_bone->get_global_transform() * physical_bone->get_joint_offset(),
+					pb->get_global_transform(),
+					pbp->get_global_transform(),
+					cjd->swing_span,
+					cjd->twist_span,
+					&points,
+					&points);
+		} break;
+		case PhysicalBone3D::JOINT_TYPE_HINGE: {
+			const PhysicalBone3D::HingeJointData *hjd(static_cast<const PhysicalBone3D::HingeJointData *>(physical_bone->get_joint_data()));
+			Joint3DGizmoPlugin::CreateHingeJointGizmo(
+					physical_bone->get_joint_offset(),
+					physical_bone->get_global_transform() * physical_bone->get_joint_offset(),
+					pb->get_global_transform(),
+					pbp->get_global_transform(),
+					hjd->angular_limit_lower,
+					hjd->angular_limit_upper,
+					hjd->angular_limit_enabled,
+					points,
+					&points,
+					&points);
+		} break;
+		case PhysicalBone3D::JOINT_TYPE_SLIDER: {
+			const PhysicalBone3D::SliderJointData *sjd(static_cast<const PhysicalBone3D::SliderJointData *>(physical_bone->get_joint_data()));
+			Joint3DGizmoPlugin::CreateSliderJointGizmo(
+					physical_bone->get_joint_offset(),
+					physical_bone->get_global_transform() * physical_bone->get_joint_offset(),
+					pb->get_global_transform(),
+					pbp->get_global_transform(),
+					sjd->angular_limit_lower,
+					sjd->angular_limit_upper,
+					sjd->linear_limit_lower,
+					sjd->linear_limit_upper,
+					points,
+					&points,
+					&points);
+		} break;
+		case PhysicalBone3D::JOINT_TYPE_6DOF: {
+			const PhysicalBone3D::SixDOFJointData *sdofjd(static_cast<const PhysicalBone3D::SixDOFJointData *>(physical_bone->get_joint_data()));
+			Joint3DGizmoPlugin::CreateGeneric6DOFJointGizmo(
+					physical_bone->get_joint_offset(),
+
+					physical_bone->get_global_transform() * physical_bone->get_joint_offset(),
+					pb->get_global_transform(),
+					pbp->get_global_transform(),
+
+					sdofjd->axis_data[0].angular_limit_lower,
+					sdofjd->axis_data[0].angular_limit_upper,
+					sdofjd->axis_data[0].linear_limit_lower,
+					sdofjd->axis_data[0].linear_limit_upper,
+					sdofjd->axis_data[0].angular_limit_enabled,
+					sdofjd->axis_data[0].linear_limit_enabled,
+
+					sdofjd->axis_data[1].angular_limit_lower,
+					sdofjd->axis_data[1].angular_limit_upper,
+					sdofjd->axis_data[1].linear_limit_lower,
+					sdofjd->axis_data[1].linear_limit_upper,
+					sdofjd->axis_data[1].angular_limit_enabled,
+					sdofjd->axis_data[1].linear_limit_enabled,
+
+					sdofjd->axis_data[2].angular_limit_lower,
+					sdofjd->axis_data[2].angular_limit_upper,
+					sdofjd->axis_data[2].linear_limit_lower,
+					sdofjd->axis_data[2].linear_limit_upper,
+					sdofjd->axis_data[2].angular_limit_enabled,
+					sdofjd->axis_data[2].linear_limit_enabled,
+
+					points,
+					&points,
+					&points);
+		} break;
+		default:
+			return;
+	}
+
+	Ref<Material> material = get_material("joint_material", p_gizmo);
+
+	p_gizmo->add_collision_segments(points);
+	p_gizmo->add_lines(points, material);
+}

editor/scene/3d/gizmos/physics/physics_bone_3d_gizmo_plugin.h

@@ -0,0 +1,45 @@
+/**************************************************************************/
+/*  physics_bone_3d_gizmo_plugin.h                                        */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "editor/scene/3d/node_3d_editor_gizmos.h"
+
+class PhysicalBone3DGizmoPlugin : public EditorNode3DGizmoPlugin {
+	GDCLASS(PhysicalBone3DGizmoPlugin, EditorNode3DGizmoPlugin);
+
+public:
+	bool has_gizmo(Node3D *p_spatial) override;
+	String get_gizmo_name() const override;
+	int get_priority() const override;
+	void redraw(EditorNode3DGizmo *p_gizmo) override;
+
+	PhysicalBone3DGizmoPlugin();
+};

editor/scene/3d/gizmos/physics/ray_cast_3d_gizmo_plugin.cpp

@@ -0,0 +1,69 @@
+/**************************************************************************/
+/*  ray_cast_3d_gizmo_plugin.cpp                                          */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "ray_cast_3d_gizmo_plugin.h"
+
+#include "scene/3d/physics/ray_cast_3d.h"
+
+RayCast3DGizmoPlugin::RayCast3DGizmoPlugin() {
+	const Color gizmo_color = SceneTree::get_singleton()->get_debug_collisions_color();
+	create_material("shape_material", gizmo_color);
+	const float gizmo_value = gizmo_color.get_v();
+	const Color gizmo_color_disabled = Color(gizmo_value, gizmo_value, gizmo_value, 0.65);
+	create_material("shape_material_disabled", gizmo_color_disabled);
+}
+
+bool RayCast3DGizmoPlugin::has_gizmo(Node3D *p_spatial) {
+	return Object::cast_to<RayCast3D>(p_spatial) != nullptr;
+}
+
+String RayCast3DGizmoPlugin::get_gizmo_name() const {
+	return "RayCast3D";
+}
+
+int RayCast3DGizmoPlugin::get_priority() const {
+	return -1;
+}
+
+void RayCast3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) {
+	RayCast3D *raycast = Object::cast_to<RayCast3D>(p_gizmo->get_node_3d());
+
+	p_gizmo->clear();
+
+	const Ref<StandardMaterial3D> material = raycast->is_enabled() ? raycast->get_debug_material() : get_material("shape_material_disabled");
+
+	p_gizmo->add_lines(raycast->get_debug_line_vertices(), material);
+
+	if (raycast->get_debug_shape_thickness() > 1) {
+		p_gizmo->add_vertices(raycast->get_debug_shape_vertices(), material, Mesh::PRIMITIVE_TRIANGLE_STRIP);
+	}
+
+	p_gizmo->add_collision_segments(raycast->get_debug_line_vertices());
+}

editor/scene/3d/gizmos/physics/ray_cast_3d_gizmo_plugin.h

@@ -0,0 +1,45 @@
+/**************************************************************************/
+/*  ray_cast_3d_gizmo_plugin.h                                            */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "editor/scene/3d/node_3d_editor_gizmos.h"
+
+class RayCast3DGizmoPlugin : public EditorNode3DGizmoPlugin {
+	GDCLASS(RayCast3DGizmoPlugin, EditorNode3DGizmoPlugin);
+
+public:
+	bool has_gizmo(Node3D *p_spatial) override;
+	String get_gizmo_name() const override;
+	int get_priority() const override;
+	void redraw(EditorNode3DGizmo *p_gizmo) override;
+
+	RayCast3DGizmoPlugin();
+};

editor/scene/3d/gizmos/physics/shape_cast_3d_gizmo_plugin.cpp

@@ -0,0 +1,69 @@
+/**************************************************************************/
+/*  shape_cast_3d_gizmo_plugin.cpp                                        */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "shape_cast_3d_gizmo_plugin.h"
+
+#include "scene/3d/physics/shape_cast_3d.h"
+
+ShapeCast3DGizmoPlugin::ShapeCast3DGizmoPlugin() {
+	const Color gizmo_color = SceneTree::get_singleton()->get_debug_collisions_color();
+	create_material("shape_material", gizmo_color);
+	const float gizmo_value = gizmo_color.get_v();
+	const Color gizmo_color_disabled = Color(gizmo_value, gizmo_value, gizmo_value, 0.65);
+	create_material("shape_material_disabled", gizmo_color_disabled);
+}
+
+bool ShapeCast3DGizmoPlugin::has_gizmo(Node3D *p_spatial) {
+	return Object::cast_to<ShapeCast3D>(p_spatial) != nullptr;
+}
+
+String ShapeCast3DGizmoPlugin::get_gizmo_name() const {
+	return "ShapeCast3D";
+}
+
+int ShapeCast3DGizmoPlugin::get_priority() const {
+	return -1;
+}
+
+void ShapeCast3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) {
+	ShapeCast3D *shapecast = Object::cast_to<ShapeCast3D>(p_gizmo->get_node_3d());
+
+	p_gizmo->clear();
+
+	const Ref<StandardMaterial3D> material = shapecast->is_enabled() ? shapecast->get_debug_material() : get_material("shape_material_disabled");
+
+	p_gizmo->add_lines(shapecast->get_debug_line_vertices(), material);
+
+	if (shapecast->get_shape().is_valid()) {
+		p_gizmo->add_lines(shapecast->get_debug_shape_vertices(), material);
+	}
+
+	p_gizmo->add_collision_segments(shapecast->get_debug_line_vertices());
+}

editor/scene/3d/gizmos/physics/shape_cast_3d_gizmo_plugin.h

@@ -0,0 +1,45 @@
+/**************************************************************************/
+/*  shape_cast_3d_gizmo_plugin.h                                          */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "editor/scene/3d/node_3d_editor_gizmos.h"
+
+class ShapeCast3DGizmoPlugin : public EditorNode3DGizmoPlugin {
+	GDCLASS(ShapeCast3DGizmoPlugin, EditorNode3DGizmoPlugin);
+
+public:
+	bool has_gizmo(Node3D *p_spatial) override;
+	String get_gizmo_name() const override;
+	int get_priority() const override;
+	void redraw(EditorNode3DGizmo *p_gizmo) override;
+
+	ShapeCast3DGizmoPlugin();
+};

editor/scene/3d/gizmos/physics/soft_body_3d_gizmo_plugin.cpp

@@ -0,0 +1,124 @@
+/**************************************************************************/
+/*  soft_body_3d_gizmo_plugin.cpp                                         */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "soft_body_3d_gizmo_plugin.h"
+
+#include "editor/editor_undo_redo_manager.h"
+#include "scene/3d/physics/soft_body_3d.h"
+
+SoftBody3DGizmoPlugin::SoftBody3DGizmoPlugin() {
+	Color gizmo_color = SceneTree::get_singleton()->get_debug_collisions_color();
+	create_material("shape_material", gizmo_color);
+	create_handle_material("handles");
+}
+
+bool SoftBody3DGizmoPlugin::has_gizmo(Node3D *p_spatial) {
+	return Object::cast_to<SoftBody3D>(p_spatial) != nullptr;
+}
+
+String SoftBody3DGizmoPlugin::get_gizmo_name() const {
+	return "SoftBody3D";
+}
+
+int SoftBody3DGizmoPlugin::get_priority() const {
+	return -1;
+}
+
+bool SoftBody3DGizmoPlugin::is_selectable_when_hidden() const {
+	return true;
+}
+
+bool SoftBody3DGizmoPlugin::can_commit_handle_on_click() const {
+	return true;
+}
+
+void SoftBody3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) {
+	SoftBody3D *soft_body = Object::cast_to<SoftBody3D>(p_gizmo->get_node_3d());
+
+	p_gizmo->clear();
+
+	if (!soft_body || soft_body->get_mesh().is_null()) {
+		return;
+	}
+
+	// find mesh
+
+	Vector<Vector3> lines;
+
+	soft_body->get_mesh()->generate_debug_mesh_lines(lines);
+
+	if (!lines.size()) {
+		return;
+	}
+
+	Ref<TriangleMesh> tm = soft_body->get_mesh()->generate_triangle_mesh();
+
+	Vector<Vector3> points;
+	for (int i = 0; i < soft_body->get_mesh()->get_surface_count(); i++) {
+		Array arrays = soft_body->get_mesh()->surface_get_arrays(i);
+		ERR_CONTINUE(arrays.is_empty());
+
+		const Vector<Vector3> &vertices = arrays[Mesh::ARRAY_VERTEX];
+		points.append_array(vertices);
+	}
+
+	Ref<Material> material = get_material("shape_material", p_gizmo);
+
+	p_gizmo->add_lines(lines, material);
+	p_gizmo->add_handles(points, get_material("handles"));
+	p_gizmo->add_collision_triangles(tm);
+}
+
+String SoftBody3DGizmoPlugin::get_handle_name(const EditorNode3DGizmo *p_gizmo, int p_id, bool p_secondary) const {
+	return "SoftBody3D pin point";
+}
+
+Variant SoftBody3DGizmoPlugin::get_handle_value(const EditorNode3DGizmo *p_gizmo, int p_id, bool p_secondary) const {
+	SoftBody3D *soft_body = Object::cast_to<SoftBody3D>(p_gizmo->get_node_3d());
+	return Variant(soft_body->is_point_pinned(p_id));
+}
+
+void SoftBody3DGizmoPlugin::commit_handle(const EditorNode3DGizmo *p_gizmo, int p_id, bool p_secondary, const Variant &p_restore, bool p_cancel) {
+	SoftBody3D *soft_body = Object::cast_to<SoftBody3D>(p_gizmo->get_node_3d());
+	const bool is_pinned = soft_body->is_point_pinned(p_id);
+
+	EditorUndoRedoManager *undo_redo = EditorUndoRedoManager::get_singleton();
+	undo_redo->create_action(vformat(is_pinned ? TTR("Remove SoftBody3D pinned point %d") : TTR("Add SoftBody3D pinned point %d"), p_id));
+	undo_redo->add_do_method(soft_body, "set_point_pinned", p_id, !is_pinned);
+	undo_redo->add_do_method(soft_body, "update_gizmos");
+	undo_redo->add_undo_method(soft_body, "set_point_pinned", p_id, is_pinned);
+	undo_redo->add_undo_method(soft_body, "update_gizmos");
+	undo_redo->commit_action();
+}
+
+bool SoftBody3DGizmoPlugin::is_handle_highlighted(const EditorNode3DGizmo *p_gizmo, int p_id, bool p_secondary) const {
+	SoftBody3D *soft_body = Object::cast_to<SoftBody3D>(p_gizmo->get_node_3d());
+	return soft_body->is_point_pinned(p_id);
+}

editor/scene/3d/gizmos/physics/soft_body_3d_gizmo_plugin.h

@@ -0,0 +1,52 @@
+/**************************************************************************/
+/*  soft_body_3d_gizmo_plugin.h                                           */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "editor/scene/3d/node_3d_editor_gizmos.h"
+
+class SoftBody3DGizmoPlugin : public EditorNode3DGizmoPlugin {
+	GDCLASS(SoftBody3DGizmoPlugin, EditorNode3DGizmoPlugin);
+
+public:
+	bool has_gizmo(Node3D *p_spatial) override;
+	String get_gizmo_name() const override;
+	int get_priority() const override;
+	bool is_selectable_when_hidden() const override;
+	bool can_commit_handle_on_click() const override;
+	void redraw(EditorNode3DGizmo *p_gizmo) override;
+
+	String get_handle_name(const EditorNode3DGizmo *p_gizmo, int p_id, bool p_secondary) const override;
+	Variant get_handle_value(const EditorNode3DGizmo *p_gizmo, int p_id, bool p_secondary) const override;
+	void commit_handle(const EditorNode3DGizmo *p_gizmo, int p_id, bool p_secondary, const Variant &p_restore, bool p_cancel = false) override;
+	bool is_handle_highlighted(const EditorNode3DGizmo *p_gizmo, int p_id, bool p_secondary) const override;
+
+	SoftBody3DGizmoPlugin();
+};

editor/scene/3d/gizmos/physics/spring_arm_3d_gizmo_plugin.cpp

@@ -0,0 +1,66 @@
+/**************************************************************************/
+/*  spring_arm_3d_gizmo_plugin.cpp                                        */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "spring_arm_3d_gizmo_plugin.h"
+
+#include "scene/3d/physics/spring_arm_3d.h"
+
+void SpringArm3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) {
+	SpringArm3D *spring_arm = Object::cast_to<SpringArm3D>(p_gizmo->get_node_3d());
+
+	p_gizmo->clear();
+
+	Vector<Vector3> lines = {
+		Vector3(),
+		Vector3(0, 0, 1.0) * spring_arm->get_length()
+	};
+
+	Ref<StandardMaterial3D> material = get_material("shape_material", p_gizmo);
+
+	p_gizmo->add_lines(lines, material);
+	p_gizmo->add_collision_segments(lines);
+}
+
+SpringArm3DGizmoPlugin::SpringArm3DGizmoPlugin() {
+	Color gizmo_color = SceneTree::get_singleton()->get_debug_collisions_color();
+	create_material("shape_material", gizmo_color);
+}
+
+bool SpringArm3DGizmoPlugin::has_gizmo(Node3D *p_spatial) {
+	return Object::cast_to<SpringArm3D>(p_spatial) != nullptr;
+}
+
+String SpringArm3DGizmoPlugin::get_gizmo_name() const {
+	return "SpringArm3D";
+}
+
+int SpringArm3DGizmoPlugin::get_priority() const {
+	return -1;
+}

editor/scene/3d/gizmos/physics/spring_arm_3d_gizmo_plugin.h

@@ -0,0 +1,45 @@
+/**************************************************************************/
+/*  spring_arm_3d_gizmo_plugin.h                                          */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "editor/scene/3d/node_3d_editor_gizmos.h"
+
+class SpringArm3DGizmoPlugin : public EditorNode3DGizmoPlugin {
+	GDCLASS(SpringArm3DGizmoPlugin, EditorNode3DGizmoPlugin);
+
+public:
+	bool has_gizmo(Node3D *p_spatial) override;
+	String get_gizmo_name() const override;
+	int get_priority() const override;
+	void redraw(EditorNode3DGizmo *p_gizmo) override;
+
+	SpringArm3DGizmoPlugin();
+};

editor/scene/3d/gizmos/physics/vehicle_body_3d_gizmo_plugin.cpp

@@ -0,0 +1,102 @@
+/**************************************************************************/
+/*  vehicle_body_3d_gizmo_plugin.cpp                                      */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "vehicle_body_3d_gizmo_plugin.h"
+
+#include "scene/3d/physics/vehicle_body_3d.h"
+
+VehicleWheel3DGizmoPlugin::VehicleWheel3DGizmoPlugin() {
+	Color gizmo_color = SceneTree::get_singleton()->get_debug_collisions_color();
+	create_material("shape_material", gizmo_color);
+}
+
+bool VehicleWheel3DGizmoPlugin::has_gizmo(Node3D *p_spatial) {
+	return Object::cast_to<VehicleWheel3D>(p_spatial) != nullptr;
+}
+
+String VehicleWheel3DGizmoPlugin::get_gizmo_name() const {
+	return "VehicleWheel3D";
+}
+
+int VehicleWheel3DGizmoPlugin::get_priority() const {
+	return -1;
+}
+
+void VehicleWheel3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) {
+	VehicleWheel3D *car_wheel = Object::cast_to<VehicleWheel3D>(p_gizmo->get_node_3d());
+
+	p_gizmo->clear();
+
+	Vector<Vector3> points;
+
+	float r = car_wheel->get_radius();
+	const int skip = 10;
+	for (int i = 0; i <= 360; i += skip) {
+		float ra = Math::deg_to_rad((float)i);
+		float rb = Math::deg_to_rad((float)i + skip);
+		Point2 a = Vector2(Math::sin(ra), Math::cos(ra)) * r;
+		Point2 b = Vector2(Math::sin(rb), Math::cos(rb)) * r;
+
+		points.push_back(Vector3(0, a.x, a.y));
+		points.push_back(Vector3(0, b.x, b.y));
+
+		const int springsec = 4;
+
+		for (int j = 0; j < springsec; j++) {
+			float t = car_wheel->get_suspension_rest_length() * 5;
+			points.push_back(Vector3(a.x, i / 360.0 * t / springsec + j * (t / springsec), a.y) * 0.2);
+			points.push_back(Vector3(b.x, (i + skip) / 360.0 * t / springsec + j * (t / springsec), b.y) * 0.2);
+		}
+	}
+
+	//travel
+	points.push_back(Vector3(0, 0, 0));
+	points.push_back(Vector3(0, car_wheel->get_suspension_rest_length(), 0));
+
+	//axis
+	points.push_back(Vector3(r * 0.2, car_wheel->get_suspension_rest_length(), 0));
+	points.push_back(Vector3(-r * 0.2, car_wheel->get_suspension_rest_length(), 0));
+	//axis
+	points.push_back(Vector3(r * 0.2, 0, 0));
+	points.push_back(Vector3(-r * 0.2, 0, 0));
+
+	//forward line
+	points.push_back(Vector3(0, -r, 0));
+	points.push_back(Vector3(0, -r, r * 2));
+	points.push_back(Vector3(0, -r, r * 2));
+	points.push_back(Vector3(r * 2 * 0.2, -r, r * 2 * 0.8));
+	points.push_back(Vector3(0, -r, r * 2));
+	points.push_back(Vector3(-r * 2 * 0.2, -r, r * 2 * 0.8));
+
+	Ref<Material> material = get_material("shape_material", p_gizmo);
+
+	p_gizmo->add_lines(points, material);
+	p_gizmo->add_collision_segments(points);
+}

editor/scene/3d/gizmos/physics/vehicle_body_3d_gizmo_plugin.h

@@ -0,0 +1,45 @@
+/**************************************************************************/
+/*  vehicle_body_3d_gizmo_plugin.h                                        */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "editor/scene/3d/node_3d_editor_gizmos.h"
+
+class VehicleWheel3DGizmoPlugin : public EditorNode3DGizmoPlugin {
+	GDCLASS(VehicleWheel3DGizmoPlugin, EditorNode3DGizmoPlugin);
+
+public:
+	bool has_gizmo(Node3D *p_spatial) override;
+	String get_gizmo_name() const override;
+	int get_priority() const override;
+	void redraw(EditorNode3DGizmo *p_gizmo) override;
+
+	VehicleWheel3DGizmoPlugin();
+};