Fixed SoftBody pinned point offset calculation

2 files changed, 60 insertions, 31 deletions

diff --git a/scene/3d/soft_body.cpp b/scene/3d/soft_body.cpp
index 4ab7013707..980c348c9b 100644
--- a/scene/3d/soft_body.cpp
+++ b/scene/3d/soft_body.cpp
@@ -133,8 +133,8 @@ bool SoftBody::_get(const StringName &p_name, Variant &r_ret) const {
 
 	if ("pinned_points" == which) {
 		Array arr_ret;
-		const int pinned_points_indices_size = pinned_points_indices.size();
-		PoolVector<PinnedPoint>::Read r = pinned_points_indices.read();
+		const int pinned_points_indices_size = pinned_points.size();
+		PoolVector<PinnedPoint>::Read r = pinned_points.read();
 		arr_ret.resize(pinned_points_indices_size);
 
 		for (int i = 0; i < pinned_points_indices_size; ++i) {
@@ -157,7 +157,7 @@ bool SoftBody::_get(const StringName &p_name, Variant &r_ret) const {
 
 void SoftBody::_get_property_list(List<PropertyInfo> *p_list) const {
 
-	const int pinned_points_indices_size = pinned_points_indices.size();
+	const int pinned_points_indices_size = pinned_points.size();
 
 	p_list->push_back(PropertyInfo(Variant::POOL_INT_ARRAY, "pinned_points"));
 
@@ -173,17 +173,17 @@ bool SoftBody::_set_property_pinned_points_indices(const Array &p_indices) {
 	const int p_indices_size = p_indices.size();
 
 	{ // Remove the pined points on physics server that will be removed by resize
-		PoolVector<PinnedPoint>::Read r = pinned_points_indices.read();
-		if (p_indices_size < pinned_points_indices.size()) {
-			for (int i = pinned_points_indices.size() - 1; i >= p_indices_size; --i) {
+		PoolVector<PinnedPoint>::Read r = pinned_points.read();
+		if (p_indices_size < pinned_points.size()) {
+			for (int i = pinned_points.size() - 1; i >= p_indices_size; --i) {
 				pin_point(r[i].point_index, false);
 			}
 		}
 	}
 
-	pinned_points_indices.resize(p_indices_size);
+	pinned_points.resize(p_indices_size);
 
-	PoolVector<PinnedPoint>::Write w = pinned_points_indices.write();
+	PoolVector<PinnedPoint>::Write w = pinned_points.write();
 	int point_index;
 	for (int i = 0; i < p_indices_size; ++i) {
 		point_index = p_indices.get(i);
@@ -198,16 +198,16 @@ bool SoftBody::_set_property_pinned_points_indices(const Array &p_indices) {
 }
 
 bool SoftBody::_set_property_pinned_points_attachment(int p_item, const String &p_what, const Variant &p_value) {
-	if (pinned_points_indices.size() <= p_item) {
+	if (pinned_points.size() <= p_item) {
 		return false;
 	}
 
 	if ("spatial_attachment_path" == p_what) {
-		PoolVector<PinnedPoint>::Write w = pinned_points_indices.write();
+		PoolVector<PinnedPoint>::Write w = pinned_points.write();
 		pin_point(w[p_item].point_index, true, p_value);
 		_make_cache_dirty();
 	} else if ("offset" == p_what) {
-		PoolVector<PinnedPoint>::Write w = pinned_points_indices.write();
+		PoolVector<PinnedPoint>::Write w = pinned_points.write();
 		w[p_item].offset = p_value;
 	} else {
 		return false;
@@ -217,10 +217,10 @@ bool SoftBody::_set_property_pinned_points_attachment(int p_item, const String &
 }
 
 bool SoftBody::_get_property_pinned_points(int p_item, const String &p_what, Variant &r_ret) const {
-	if (pinned_points_indices.size() <= p_item) {
+	if (pinned_points.size() <= p_item) {
 		return false;
 	}
-	PoolVector<PinnedPoint>::Read r = pinned_points_indices.read();
+	PoolVector<PinnedPoint>::Read r = pinned_points.read();
 
 	if ("point_index" == p_what) {
 		r_ret = r[p_item].point_index;
@@ -236,6 +236,8 @@ bool SoftBody::_get_property_pinned_points(int p_item, const String &p_what, Var
 }
 
 void SoftBody::_changed_callback(Object *p_changed, const char *p_prop) {
+	update_physics_server();
+	_reset_points_offsets();
 #ifdef TOOLS_ENABLED
 	if (p_changed == this) {
 		update_configuration_warning();
@@ -247,8 +249,10 @@ void SoftBody::_notification(int p_what) {
 	switch (p_what) {
 		case NOTIFICATION_ENTER_WORLD: {
 
-			if (Engine::get_singleton()->is_editor_hint())
+			if (Engine::get_singleton()->is_editor_hint()) {
+
 				add_change_receptor(this);
+			}
 
 			RID space = get_world()->get_space();
 			PhysicsServer::get_singleton()->soft_body_set_space(physics_rid, space);
@@ -261,8 +265,10 @@ void SoftBody::_notification(int p_what) {
 		} break;
 		case NOTIFICATION_TRANSFORM_CHANGED: {
 
-			if (Engine::get_singleton()->is_editor_hint())
+			if (Engine::get_singleton()->is_editor_hint()) {
+				_reset_points_offsets();
 				return;
+			}
 
 			PhysicsServer::get_singleton()->soft_body_set_transform(physics_rid, get_global_transform());
 
@@ -280,8 +286,8 @@ void SoftBody::_notification(int p_what) {
 
 			_update_cache_pin_points_datas();
 			// Submit bone attachment
-			const int pinned_points_indices_size = pinned_points_indices.size();
-			PoolVector<PinnedPoint>::Read r = pinned_points_indices.read();
+			const int pinned_points_indices_size = pinned_points.size();
+			PoolVector<PinnedPoint>::Read r = pinned_points.read();
 			for (int i = 0; i < pinned_points_indices_size; ++i) {
 				if (r[i].spatial_attachment) {
 					PhysicsServer::get_singleton()->soft_body_move_point(physics_rid, r[i].point_index, r[i].spatial_attachment->get_global_transform().xform(r[i].offset));
@@ -531,15 +537,15 @@ const NodePath &SoftBody::get_parent_collision_ignore() const {
 }
 
 void SoftBody::set_pinned_points_indices(PoolVector<SoftBody::PinnedPoint> p_pinned_points_indices) {
-	pinned_points_indices = p_pinned_points_indices;
-	PoolVector<PinnedPoint>::Read w = pinned_points_indices.read();
-	for (int i = pinned_points_indices.size() - 1; 0 <= i; --i) {
+	pinned_points = p_pinned_points_indices;
+	PoolVector<PinnedPoint>::Read w = pinned_points.read();
+	for (int i = pinned_points.size() - 1; 0 <= i; --i) {
 		pin_point(p_pinned_points_indices[i].point_index, true);
 	}
 }
 
 PoolVector<SoftBody::PinnedPoint> SoftBody::get_pinned_points_indices() {
-	return pinned_points_indices;
+	return pinned_points;
 }
 
 void SoftBody::add_collision_exception_with(Node *p_node) {
@@ -685,8 +691,8 @@ SoftBody::~SoftBody() {
 
 void SoftBody::reset_softbody_pin() {
 	PhysicsServer::get_singleton()->soft_body_remove_all_pinned_points(physics_rid);
-	PoolVector<PinnedPoint>::Read pps = pinned_points_indices.read();
-	for (int i = pinned_points_indices.size() - 1; 0 < i; --i) {
+	PoolVector<PinnedPoint>::Read pps = pinned_points.read();
+	for (int i = pinned_points.size() - 1; 0 < i; --i) {
 		PhysicsServer::get_singleton()->soft_body_pin_point(physics_rid, pps[i].point_index, true);
 	}
 }
@@ -701,8 +707,8 @@ void SoftBody::_update_cache_pin_points_datas() {
 
 	pinned_points_cache_dirty = false;
 
-	PoolVector<PinnedPoint>::Write w = pinned_points_indices.write();
-	for (int i = pinned_points_indices.size() - 1; 0 <= i; --i) {
+	PoolVector<PinnedPoint>::Write w = pinned_points.write();
+	for (int i = pinned_points.size() - 1; 0 <= i; --i) {
 
 		if (!w[i].spatial_attachment_path.is_empty()) {
 			w[i].spatial_attachment = Object::cast_to<Spatial>(get_node(w[i].spatial_attachment_path));
@@ -731,7 +737,7 @@ void SoftBody::_add_pinned_point(int p_point_index, const NodePath &p_spatial_at
 			pp.offset = (pp.spatial_attachment->get_global_transform().affine_inverse() * get_global_transform()).xform(PhysicsServer::get_singleton()->soft_body_get_point_global_position(physics_rid, pp.point_index));
 		}
 
-		pinned_points_indices.push_back(pp);
+		pinned_points.push_back(pp);
 
 	} else {
 
@@ -745,10 +751,29 @@ void SoftBody::_add_pinned_point(int p_point_index, const NodePath &p_spatial_at
 	}
 }
 
+void SoftBody::_reset_points_offsets() {
+
+	if (!Engine::get_singleton()->is_editor_hint())
+		return;
+
+	PoolVector<PinnedPoint>::Read r = pinned_points.read();
+	PoolVector<PinnedPoint>::Write w = pinned_points.write();
+	for (int i = pinned_points.size() - 1; 0 <= i; --i) {
+
+		if (!r[i].spatial_attachment)
+			w[i].spatial_attachment = Object::cast_to<Spatial>(get_node(r[i].spatial_attachment_path));
+
+		if (!r[i].spatial_attachment)
+			continue;
+
+		w[i].offset = (r[i].spatial_attachment->get_global_transform().affine_inverse() * get_global_transform()).xform(PhysicsServer::get_singleton()->soft_body_get_point_global_position(physics_rid, r[i].point_index));
+	}
+}
+
 void SoftBody::_remove_pinned_point(int p_point_index) {
 	const int id(_has_pinned_point(p_point_index));
 	if (-1 != id) {
-		pinned_points_indices.remove(id);
+		pinned_points.remove(id);
 	}
 }
 
@@ -758,14 +783,14 @@ int SoftBody::_get_pinned_point(int p_point_index, SoftBody::PinnedPoint *&r_poi
 		r_point = NULL;
 		return -1;
 	} else {
-		r_point = const_cast<SoftBody::PinnedPoint *>(&pinned_points_indices.read()[id]);
+		r_point = const_cast<SoftBody::PinnedPoint *>(&pinned_points.read()[id]);
 		return id;
 	}
 }
 
 int SoftBody::_has_pinned_point(int p_point_index) const {
-	PoolVector<PinnedPoint>::Read r = pinned_points_indices.read();
-	for (int i = pinned_points_indices.size() - 1; 0 <= i; --i) {
+	PoolVector<PinnedPoint>::Read r = pinned_points.read();
+	for (int i = pinned_points.size() - 1; 0 <= i; --i) {
 		if (p_point_index == r[i].point_index) {
 			return i;
 		}
diff --git a/scene/3d/soft_body.h b/scene/3d/soft_body.h
index 6cf19ef8c4..cee32b9651 100644
--- a/scene/3d/soft_body.h
+++ b/scene/3d/soft_body.h
@@ -87,7 +87,7 @@ private:
 	uint32_t collision_mask;
 	uint32_t collision_layer;
 	NodePath parent_collision_ignore;
-	PoolVector<PinnedPoint> pinned_points_indices;
+	PoolVector<PinnedPoint> pinned_points;
 	bool simulation_started;
 	bool pinned_points_cache_dirty;
 
@@ -184,10 +184,14 @@ public:
 
 private:
 	void reset_softbody_pin();
+
 	void _make_cache_dirty();
 	void _update_cache_pin_points_datas();
+
 	void _pin_point_on_physics_server(int p_point_index, bool pin);
 	void _add_pinned_point(int p_point_index, const NodePath &p_spatial_attachment_path);
+	void _reset_points_offsets();
+
 	void _remove_pinned_point(int p_point_index);
 	int _get_pinned_point(int p_point_index, PinnedPoint *&r_point) const;
 	int _has_pinned_point(int p_point_index) const;