1 files changed, 129 insertions, 23 deletions

diff --git a/editor/spatial_editor_gizmos.cpp b/editor/spatial_editor_gizmos.cpp
index c45dea0df7..c450c0fd4c 100644
--- a/editor/spatial_editor_gizmos.cpp
+++ b/editor/spatial_editor_gizmos.cpp
@@ -33,6 +33,7 @@
 #include "geometry.h"
 #include "quick_hull.h"
 #include "scene/3d/camera.h"
+#include "scene/3d/soft_body.h"
 #include "scene/resources/box_shape.h"
 #include "scene/resources/capsule_shape.h"
 #include "scene/resources/convex_polygon_shape.h"
@@ -228,7 +229,7 @@ void EditorSpatialGizmo::add_collision_segments(const Vector<Vector3> &p_lines)
 	collision_segments.resize(from + p_lines.size());
 	for (int i = 0; i < p_lines.size(); i++) {
 
-		collision_segments[from + i] = p_lines[i];
+		collision_segments.write[from + i] = p_lines[i];
 	}
 }
 
@@ -256,8 +257,12 @@ void EditorSpatialGizmo::add_handles(const Vector<Vector3> &p_handles, bool p_bi
 		for (int i = 0; i < p_handles.size(); i++) {
 
 			Color col(1, 1, 1, 1);
+			if (is_gizmo_handle_highlighted(i))
+				col = Color(0, 0, 1, 0.9);
+
 			if (SpatialEditor::get_singleton()->get_over_gizmo_handle() != i)
-				col = Color(0.9, 0.9, 0.9, 0.9);
+				col.a = 0.8;
+
 			w[i] = col;
 		}
 	}
@@ -291,14 +296,14 @@ void EditorSpatialGizmo::add_handles(const Vector<Vector3> &p_handles, bool p_bi
 		int chs = handles.size();
 		handles.resize(chs + p_handles.size());
 		for (int i = 0; i < p_handles.size(); i++) {
-			handles[i + chs] = p_handles[i];
+			handles.write[i + chs] = p_handles[i];
 		}
 	} else {
 
 		int chs = secondary_handles.size();
 		secondary_handles.resize(chs + p_handles.size());
 		for (int i = 0; i < p_handles.size(); i++) {
-			secondary_handles[i + chs] = p_handles[i];
+			secondary_handles.write[i + chs] = p_handles[i];
 		}
 	}
 }
@@ -592,7 +597,7 @@ void EditorSpatialGizmo::create() {
 
 	for (int i = 0; i < instances.size(); i++) {
 
-		instances[i].create_instance(spatial_node);
+		instances.write[i].create_instance(spatial_node);
 	}
 
 	transform();
@@ -616,7 +621,7 @@ void EditorSpatialGizmo::free() {
 
 		if (instances[i].instance.is_valid())
 			VS::get_singleton()->free(instances[i].instance);
-		instances[i].instance = RID();
+		instances.write[i].instance = RID();
 	}
 
 	valid = false;
@@ -1119,8 +1124,8 @@ void AudioStreamPlayer3DSpatialGizmo::redraw() {
 			Vector3 from(Math::sin(a) * radius, Math::cos(a) * radius, ofs);
 			Vector3 to(Math::sin(an) * radius, Math::cos(an) * radius, ofs);
 
-			points[i * 2 + 0] = from;
-			points[i * 2 + 1] = to;
+			points.write[i * 2 + 0] = from;
+			points.write[i * 2 + 1] = to;
 		}
 
 		for (int i = 0; i < 4; i++) {
@@ -1129,8 +1134,8 @@ void AudioStreamPlayer3DSpatialGizmo::redraw() {
 
 			Vector3 from(Math::sin(a) * radius, Math::cos(a) * radius, ofs);
 
-			points[200 + i * 2 + 0] = from;
-			points[200 + i * 2 + 1] = Vector3();
+			points.write[200 + i * 2 + 0] = from;
+			points.write[200 + i * 2 + 1] = Vector3();
 		}
 
 		add_lines(points, material);
@@ -1436,11 +1441,11 @@ void SkeletonSpatialGizmo::redraw() {
 	weights.resize(4);
 
 	for (int i = 0; i < 4; i++) {
-		bones[i] = 0;
-		weights[i] = 0;
+		bones.write[i] = 0;
+		weights.write[i] = 0;
 	}
 
-	weights[0] = 1;
+	weights.write[0] = 1;
 
 	AABB aabb;
 
@@ -1452,7 +1457,7 @@ void SkeletonSpatialGizmo::redraw() {
 		int parent = skel->get_bone_parent(i);
 
 		if (parent >= 0) {
-			grests[i] = grests[parent] * skel->get_bone_rest(i);
+			grests.write[i] = grests[parent] * skel->get_bone_rest(i);
 
 			Vector3 v0 = grests[parent].origin;
 			Vector3 v1 = grests[i].origin;
@@ -1475,7 +1480,7 @@ void SkeletonSpatialGizmo::redraw() {
 			int pointidx = 0;
 			for (int j = 0; j < 3; j++) {
 
-				bones[0] = parent;
+				bones.write[0] = parent;
 				surface_tool->add_bones(bones);
 				surface_tool->add_weights(weights);
 				surface_tool->add_color(rootcolor);
@@ -1503,7 +1508,7 @@ void SkeletonSpatialGizmo::redraw() {
 					Vector3 point = v0 + d * dist * 0.2;
 					point += axis * dist * 0.1;
 
-					bones[0] = parent;
+					bones.write[0] = parent;
 					surface_tool->add_bones(bones);
 					surface_tool->add_weights(weights);
 					surface_tool->add_color(bonecolor);
@@ -1513,12 +1518,12 @@ void SkeletonSpatialGizmo::redraw() {
 					surface_tool->add_color(bonecolor);
 					surface_tool->add_vertex(point);
 
-					bones[0] = parent;
+					bones.write[0] = parent;
 					surface_tool->add_bones(bones);
 					surface_tool->add_weights(weights);
 					surface_tool->add_color(bonecolor);
 					surface_tool->add_vertex(point);
-					bones[0] = i;
+					bones.write[0] = i;
 					surface_tool->add_bones(bones);
 					surface_tool->add_weights(weights);
 					surface_tool->add_color(bonecolor);
@@ -1530,7 +1535,7 @@ void SkeletonSpatialGizmo::redraw() {
 			SWAP(points[1], points[2]);
 			for (int j = 0; j < 4; j++) {
 
-				bones[0] = parent;
+				bones.write[0] = parent;
 				surface_tool->add_bones(bones);
 				surface_tool->add_weights(weights);
 				surface_tool->add_color(bonecolor);
@@ -1555,8 +1560,8 @@ void SkeletonSpatialGizmo::redraw() {
 */
 		} else {
 
-			grests[i] = skel->get_bone_rest(i);
-			bones[0] = i;
+			grests.write[i] = skel->get_bone_rest(i);
+			bones.write[0] = i;
 		}
 		/*
 		Transform  t = grests[i];
@@ -1914,6 +1919,100 @@ VehicleWheelSpatialGizmo::VehicleWheelSpatialGizmo(VehicleWheel *p_car_wheel) {
 
 ///////////
 
+void SoftBodySpatialGizmo::redraw() {
+	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;
+	}
+
+	Vector<Vector3> points;
+	soft_body->get_mesh()->generate_debug_mesh_indices(points);
+
+	soft_body->get_mesh()->clear_cache();
+
+	Color gizmo_color = EDITOR_GET("editors/3d_gizmos/gizmo_colors/shape");
+	Ref<Material> material = create_material("shape_material", gizmo_color);
+
+	add_lines(lines, material);
+	add_collision_segments(lines);
+	add_handles(points);
+}
+
+bool SoftBodySpatialGizmo::intersect_ray(Camera *p_camera, const Point2 &p_point, Vector3 &r_pos, Vector3 &r_normal, int *r_gizmo_handle, bool p_sec_first) {
+	return EditorSpatialGizmo::intersect_ray(p_camera, p_point, r_pos, r_normal, r_gizmo_handle, p_sec_first);
+
+	/* Perform a shape cast but doesn't work with softbody
+	PhysicsDirectSpaceState *space_state = PhysicsServer::get_singleton()->space_get_direct_state(SceneTree::get_singleton()->get_root()->get_world()->get_space());
+	if (!physics_sphere_shape.is_valid()) {
+		physics_sphere_shape = PhysicsServer::get_singleton()->shape_create(PhysicsServer::SHAPE_SPHERE);
+		real_t radius = 0.02;
+		PhysicsServer::get_singleton()->shape_set_data(physics_sphere_shape, radius);
+	}
+
+	Vector3 sphere_motion(p_camera->project_ray_normal(p_point));
+	real_t closest_safe;
+	real_t closest_unsafe;
+	PhysicsDirectSpaceState::ShapeRestInfo result;
+	bool collided = space_state->cast_motion(
+			physics_sphere_shape,
+			p_camera->get_transform(),
+			sphere_motion * Vector3(1000, 1000, 1000),
+			0.f,
+			closest_safe,
+			closest_unsafe,
+			Set<RID>(),
+			0xFFFFFFFF,
+			0xFFFFFFFF,
+			&result);
+
+	if (collided) {
+
+		if (result.collider_id == soft_body->get_instance_id()) {
+			print_line("Collided");
+		} else {
+			print_line("Collided but with wrong object: " + itos(result.collider_id));
+		}
+	} else {
+		print_line("Not collided, motion: x: " + rtos(sphere_motion[0]) + " y: " + rtos(sphere_motion[1]) + " z: " + rtos(sphere_motion[2]));
+	}
+	return false;
+	*/
+}
+
+void SoftBodySpatialGizmo::commit_handle(int p_idx, const Variant &p_restore, bool p_cancel) {
+	soft_body->pin_point_toggle(p_idx);
+	redraw();
+}
+
+bool SoftBodySpatialGizmo::is_gizmo_handle_highlighted(int idx) const {
+	return soft_body->is_point_pinned(idx);
+}
+
+SoftBodySpatialGizmo::SoftBodySpatialGizmo(SoftBody *p_soft_physics_body) :
+		EditorSpatialGizmo(),
+		soft_body(p_soft_physics_body) {
+	set_spatial_node(p_soft_physics_body);
+}
+
+SoftBodySpatialGizmo::~SoftBodySpatialGizmo() {
+	//if (!physics_sphere_shape.is_valid()) {
+	//	PhysicsServer::get_singleton()->free(physics_sphere_shape);
+	//}
+}
+
+///////////
+
 String CollisionShapeSpatialGizmo::get_handle_name(int p_idx) const {
 
 	Ref<Shape> s = cs->get_shape();
@@ -2443,8 +2542,8 @@ void CollisionShapeSpatialGizmo::redraw() {
 				Vector<Vector3> points;
 				points.resize(md.edges.size() * 2);
 				for (int i = 0; i < md.edges.size(); i++) {
-					points[i * 2 + 0] = md.vertices[md.edges[i].a];
-					points[i * 2 + 1] = md.vertices[md.edges[i].b];
+					points.write[i * 2 + 0] = md.vertices[md.edges[i].a];
+					points.write[i * 2 + 1] = md.vertices[md.edges[i].b];
 				}
 
 				add_lines(points, material);
@@ -4051,6 +4150,12 @@ Ref<SpatialEditorGizmo> SpatialEditorGizmos::get_gizmo(Spatial *p_spatial) {
 		return lsg;
 	}
 
+	if (Object::cast_to<SoftBody>(p_spatial)) {
+
+		Ref<SoftBodySpatialGizmo> misg = memnew(SoftBodySpatialGizmo(Object::cast_to<SoftBody>(p_spatial)));
+		return misg;
+	}
+
 	if (Object::cast_to<MeshInstance>(p_spatial)) {
 
 		Ref<MeshInstanceSpatialGizmo> misg = memnew(MeshInstanceSpatialGizmo(Object::cast_to<MeshInstance>(p_spatial)));
@@ -4081,6 +4186,7 @@ Ref<SpatialEditorGizmo> SpatialEditorGizmos::get_gizmo(Spatial *p_spatial) {
 		return misg;
 	}
 */
+
 	if (Object::cast_to<CollisionShape>(p_spatial)) {
 
 		Ref<CollisionShapeSpatialGizmo> misg = memnew(CollisionShapeSpatialGizmo(Object::cast_to<CollisionShape>(p_spatial)));