1 files changed, 168 insertions, 55 deletions

diff --git a/scene/3d/skeleton_3d.cpp b/scene/3d/skeleton_3d.cpp
index db9c4db30d..9e4c9b1832 100644
--- a/scene/3d/skeleton_3d.cpp
+++ b/scene/3d/skeleton_3d.cpp
@@ -81,7 +81,7 @@ bool Skeleton3D::_set(const StringName &p_path, const Variant &p_value) {
 		return false;
 	}
 
-	int which = path.get_slicec('/', 1).to_int();
+	uint32_t which = path.get_slicec('/', 1).to_int();
 	String what = path.get_slicec('/', 2);
 
 	if (which == bones.size() && what == "name") {
@@ -89,7 +89,7 @@ bool Skeleton3D::_set(const StringName &p_path, const Variant &p_value) {
 		return true;
 	}
 
-	ERR_FAIL_INDEX_V(which, bones.size(), false);
+	ERR_FAIL_UNSIGNED_INDEX_V(which, bones.size(), false);
 
 	if (what == "parent") {
 		set_bone_parent(which, p_value);
@@ -153,10 +153,10 @@ bool Skeleton3D::_get(const StringName &p_path, Variant &r_ret) const {
 		return false;
 	}
 
-	int which = path.get_slicec('/', 1).to_int();
+	uint32_t which = path.get_slicec('/', 1).to_int();
 	String what = path.get_slicec('/', 2);
 
-	ERR_FAIL_INDEX_V(which, bones.size(), false);
+	ERR_FAIL_UNSIGNED_INDEX_V(which, bones.size(), false);
 
 	if (what == "name") {
 		r_ret = get_bone_name(which);
@@ -182,7 +182,7 @@ bool Skeleton3D::_get(const StringName &p_path, Variant &r_ret) const {
 }
 
 void Skeleton3D::_get_property_list(List<PropertyInfo> *p_list) const {
-	for (int i = 0; i < bones.size(); i++) {
+	for (uint32_t i = 0; i < bones.size(); i++) {
 		const String prep = vformat("%s/%d/", PNAME("bones"), i);
 		p_list->push_back(PropertyInfo(Variant::STRING, prep + PNAME("name"), PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR));
 		p_list->push_back(PropertyInfo(Variant::INT, prep + PNAME("parent"), PROPERTY_HINT_RANGE, "-1," + itos(bones.size() - 1) + ",1", PROPERTY_USAGE_NO_EDITOR));
@@ -241,7 +241,7 @@ void Skeleton3D::_update_process_order() {
 		return;
 	}
 
-	Bone *bonesptr = bones.ptrw();
+	Bone *bonesptr = bones.ptr();
 	int len = bones.size();
 
 	parentless_bones.clear();
@@ -276,6 +276,8 @@ void Skeleton3D::_update_process_order() {
 
 	concatenated_bone_names = StringName();
 
+	_update_bones_nested_set();
+
 	process_order_dirty = false;
 
 	emit_signal("bone_list_changed");
@@ -283,7 +285,7 @@ void Skeleton3D::_update_process_order() {
 
 void Skeleton3D::_update_bone_names() const {
 	String names;
-	for (int i = 0; i < bones.size(); i++) {
+	for (uint32_t i = 0; i < bones.size(); i++) {
 		if (i > 0) {
 			names += ",";
 		}
@@ -331,16 +333,21 @@ void Skeleton3D::_notification(int p_what) {
 
 			updating = true;
 
-			Bone *bonesptr = bones.ptrw();
+			Bone *bonesptr = bones.ptr();
 			int len = bones.size();
 
+			thread_local LocalVector<bool> bone_global_pose_dirty_backup;
+
 			// Process modifiers.
 			_find_modifiers();
 			if (!modifiers.is_empty()) {
 				// Store unmodified bone poses.
-				for (int i = 0; i < bones.size(); i++) {
+				for (uint32_t i = 0; i < bones.size(); i++) {
 					bones_backup[i].save(bones[i]);
 				}
+				// Store dirty flags for global bone poses.
+				bone_global_pose_dirty_backup = bone_global_pose_dirty;
+
 				_process_modifiers();
 			}
 
@@ -412,9 +419,11 @@ void Skeleton3D::_notification(int p_what) {
 
 			if (!modifiers.is_empty()) {
 				// Restore unmodified bone poses.
-				for (int i = 0; i < bones.size(); i++) {
-					bones_backup[i].restore(bones.write[i]);
+				for (uint32_t i = 0; i < bones.size(); i++) {
+					bones_backup[i].restore(bones[i]);
 				}
+				// Restore dirty flags for global bone poses.
+				bone_global_pose_dirty = bone_global_pose_dirty_backup;
 			}
 
 			updating = false;
@@ -457,10 +466,111 @@ void Skeleton3D::_make_modifiers_dirty() {
 	_update_deferred(UPDATE_FLAG_MODIFIER);
 }
 
+void Skeleton3D::_update_bones_nested_set() {
+	nested_set_offset_to_bone_index.resize(bones.size());
+	bone_global_pose_dirty.resize(bones.size());
+	_make_bone_global_poses_dirty();
+
+	int offset = 0;
+	for (int bone : parentless_bones) {
+		offset += _update_bone_nested_set(bone, offset);
+	}
+}
+
+int Skeleton3D::_update_bone_nested_set(int p_bone, int p_offset) {
+	Bone &bone = bones[p_bone];
+	int offset = p_offset + 1;
+	int span = 1;
+
+	for (int child_bone : bone.child_bones) {
+		int subspan = _update_bone_nested_set(child_bone, offset);
+		offset += subspan;
+		span += subspan;
+	}
+
+	nested_set_offset_to_bone_index[p_offset] = p_bone;
+	bone.nested_set_offset = p_offset;
+	bone.nested_set_span = span;
+
+	return span;
+}
+
+void Skeleton3D::_make_bone_global_poses_dirty() {
+	for (uint32_t i = 0; i < bone_global_pose_dirty.size(); i++) {
+		bone_global_pose_dirty[i] = true;
+	}
+}
+
+void Skeleton3D::_make_bone_global_pose_subtree_dirty(int p_bone) {
+	if (process_order_dirty) {
+		return;
+	}
+
+	const Bone &bone = bones[p_bone];
+	int span_offset = bone.nested_set_offset;
+	// No need to make subtree dirty when bone is already dirty.
+	if (bone_global_pose_dirty[span_offset]) {
+		return;
+	}
+
+	// Make global poses of subtree dirty.
+	int span_end = span_offset + bone.nested_set_span;
+	for (int i = span_offset; i < span_end; i++) {
+		bone_global_pose_dirty[i] = true;
+	}
+}
+
+void Skeleton3D::_update_bone_global_pose(int p_bone) {
+	const int bone_size = bones.size();
+	ERR_FAIL_INDEX(p_bone, bone_size);
+
+	_update_process_order();
+
+	// Global pose is already calculated.
+	int nested_set_offset = bones[p_bone].nested_set_offset;
+	if (!bone_global_pose_dirty[nested_set_offset]) {
+		return;
+	}
+
+	thread_local LocalVector<int> bone_list;
+	bone_list.clear();
+	Transform3D global_pose;
+
+	// Create list of parent bones for which the global pose needs to be recalculated.
+	for (int bone = p_bone; bone >= 0; bone = bones[bone].parent) {
+		int offset = bones[bone].nested_set_offset;
+		// Stop searching when global pose is not dirty.
+		if (!bone_global_pose_dirty[offset]) {
+			global_pose = bones[bone].global_pose;
+			break;
+		}
+
+		bone_list.push_back(bone);
+	}
+
+	// Calculate global poses for all parent bones and the current bone.
+	for (int i = bone_list.size() - 1; i >= 0; i--) {
+		int bone_idx = bone_list[i];
+		Bone &bone = bones[bone_idx];
+		bool bone_enabled = bone.enabled && !show_rest_only;
+		Transform3D bone_pose = bone_enabled ? get_bone_pose(bone_idx) : get_bone_rest(bone_idx);
+
+		global_pose *= bone_pose;
+#ifndef DISABLE_DEPRECATED
+		if (bone.global_pose_override_amount >= CMP_EPSILON) {
+			global_pose = global_pose.interpolate_with(bone.global_pose_override, bone.global_pose_override_amount);
+		}
+#endif // _DISABLE_DEPRECATED
+
+		bone.global_pose = global_pose;
+		bone_global_pose_dirty[bone.nested_set_offset] = false;
+	}
+}
+
 Transform3D Skeleton3D::get_bone_global_pose(int p_bone) const {
 	const int bone_size = bones.size();
 	ERR_FAIL_INDEX_V(p_bone, bone_size, Transform3D());
-	const_cast<Skeleton3D *>(this)->force_update_all_dirty_bones();
+	const_cast<Skeleton3D *>(this)->_update_bone_global_pose(p_bone);
 	return bones[p_bone].global_pose;
 }
 
@@ -534,7 +644,7 @@ void Skeleton3D::set_bone_name(int p_bone, const String &p_name) {
 	}
 
 	name_to_bone_index.erase(bones[p_bone].name);
-	bones.write[p_bone].name = p_name;
+	bones[p_bone].name = p_name;
 	name_to_bone_index.insert(p_name, p_bone);
 
 	version++;
@@ -582,13 +692,13 @@ void Skeleton3D::set_bone_meta(int p_bone, const StringName &p_key, const Varian
 	ERR_FAIL_INDEX(p_bone, bone_size);
 
 	if (p_value.get_type() == Variant::NIL) {
-		if (bones.write[p_bone].metadata.has(p_key)) {
-			bones.write[p_bone].metadata.erase(p_key);
+		if (bones[p_bone].metadata.has(p_key)) {
+			bones[p_bone].metadata.erase(p_key);
 		}
 		return;
 	}
 
-	bones.write[p_bone].metadata.insert(p_key, p_value, false);
+	bones[p_bone].metadata.insert(p_key, p_value, false);
 }
 
 bool Skeleton3D::is_bone_parent_of(int p_bone, int p_parent_bone_id) const {
@@ -615,7 +725,7 @@ void Skeleton3D::set_bone_parent(int p_bone, int p_parent) {
 	ERR_FAIL_COND(p_parent != -1 && (p_parent < 0));
 	ERR_FAIL_COND(p_bone == p_parent);
 
-	bones.write[p_bone].parent = p_parent;
+	bones[p_bone].parent = p_parent;
 	process_order_dirty = true;
 	rest_dirty = true;
 	_make_dirty();
@@ -629,11 +739,11 @@ void Skeleton3D::unparent_bone_and_rest(int p_bone) {
 
 	int parent = bones[p_bone].parent;
 	while (parent >= 0) {
-		bones.write[p_bone].rest = bones[parent].rest * bones[p_bone].rest;
+		bones[p_bone].rest = bones[parent].rest * bones[p_bone].rest;
 		parent = bones[parent].parent;
 	}
 
-	bones.write[p_bone].parent = -1;
+	bones[p_bone].parent = -1;
 	process_order_dirty = true;
 
 	rest_dirty = true;
@@ -669,9 +779,10 @@ void Skeleton3D::set_bone_rest(int p_bone, const Transform3D &p_rest) {
 	const int bone_size = bones.size();
 	ERR_FAIL_INDEX(p_bone, bone_size);
 
-	bones.write[p_bone].rest = p_rest;
+	bones[p_bone].rest = p_rest;
 	rest_dirty = true;
 	_make_dirty();
+	_make_bone_global_pose_subtree_dirty(p_bone);
 }
 Transform3D Skeleton3D::get_bone_rest(int p_bone) const {
 	const int bone_size = bones.size();
@@ -692,9 +803,10 @@ void Skeleton3D::set_bone_enabled(int p_bone, bool p_enabled) {
 	const int bone_size = bones.size();
 	ERR_FAIL_INDEX(p_bone, bone_size);
 
-	bones.write[p_bone].enabled = p_enabled;
+	bones[p_bone].enabled = p_enabled;
 	emit_signal(SceneStringName(bone_enabled_changed), p_bone);
 	_make_dirty();
+	_make_bone_global_pose_subtree_dirty(p_bone);
 }
 
 bool Skeleton3D::is_bone_enabled(int p_bone) const {
@@ -707,6 +819,7 @@ void Skeleton3D::set_show_rest_only(bool p_enabled) {
 	show_rest_only = p_enabled;
 	emit_signal(SceneStringName(show_rest_only_changed));
 	_make_dirty();
+	_make_bone_global_poses_dirty();
 }
 
 bool Skeleton3D::is_show_rest_only() const {
@@ -727,12 +840,13 @@ void Skeleton3D::set_bone_pose(int p_bone, const Transform3D &p_pose) {
 	const int bone_size = bones.size();
 	ERR_FAIL_INDEX(p_bone, bone_size);
 
-	bones.write[p_bone].pose_position = p_pose.origin;
-	bones.write[p_bone].pose_rotation = p_pose.basis.get_rotation_quaternion();
-	bones.write[p_bone].pose_scale = p_pose.basis.get_scale();
-	bones.write[p_bone].pose_cache_dirty = true;
+	bones[p_bone].pose_position = p_pose.origin;
+	bones[p_bone].pose_rotation = p_pose.basis.get_rotation_quaternion();
+	bones[p_bone].pose_scale = p_pose.basis.get_scale();
+	bones[p_bone].pose_cache_dirty = true;
 	if (is_inside_tree()) {
 		_make_dirty();
+		_make_bone_global_pose_subtree_dirty(p_bone);
 	}
 }
 
@@ -740,30 +854,33 @@ void Skeleton3D::set_bone_pose_position(int p_bone, const Vector3 &p_position) {
 	const int bone_size = bones.size();
 	ERR_FAIL_INDEX(p_bone, bone_size);
 
-	bones.write[p_bone].pose_position = p_position;
-	bones.write[p_bone].pose_cache_dirty = true;
+	bones[p_bone].pose_position = p_position;
+	bones[p_bone].pose_cache_dirty = true;
 	if (is_inside_tree()) {
 		_make_dirty();
+		_make_bone_global_pose_subtree_dirty(p_bone);
 	}
 }
 void Skeleton3D::set_bone_pose_rotation(int p_bone, const Quaternion &p_rotation) {
 	const int bone_size = bones.size();
 	ERR_FAIL_INDEX(p_bone, bone_size);
 
-	bones.write[p_bone].pose_rotation = p_rotation;
-	bones.write[p_bone].pose_cache_dirty = true;
+	bones[p_bone].pose_rotation = p_rotation;
+	bones[p_bone].pose_cache_dirty = true;
 	if (is_inside_tree()) {
 		_make_dirty();
+		_make_bone_global_pose_subtree_dirty(p_bone);
 	}
 }
 void Skeleton3D::set_bone_pose_scale(int p_bone, const Vector3 &p_scale) {
 	const int bone_size = bones.size();
 	ERR_FAIL_INDEX(p_bone, bone_size);
 
-	bones.write[p_bone].pose_scale = p_scale;
-	bones.write[p_bone].pose_cache_dirty = true;
+	bones[p_bone].pose_scale = p_scale;
+	bones[p_bone].pose_cache_dirty = true;
 	if (is_inside_tree()) {
 		_make_dirty();
+		_make_bone_global_pose_subtree_dirty(p_bone);
 	}
 }
 
@@ -794,7 +911,7 @@ void Skeleton3D::reset_bone_pose(int p_bone) {
 }
 
 void Skeleton3D::reset_bone_poses() {
-	for (int i = 0; i < bones.size(); i++) {
+	for (uint32_t i = 0; i < bones.size(); i++) {
 		reset_bone_pose(i);
 	}
 }
@@ -802,7 +919,7 @@ void Skeleton3D::reset_bone_poses() {
 Transform3D Skeleton3D::get_bone_pose(int p_bone) const {
 	const int bone_size = bones.size();
 	ERR_FAIL_INDEX_V(p_bone, bone_size, Transform3D());
-	const_cast<Skeleton3D *>(this)->bones.write[p_bone].update_pose_cache();
+	const_cast<Skeleton3D *>(this)->bones[p_bone].update_pose_cache();
 	return bones[p_bone].pose_cache;
 }
 
@@ -853,7 +970,7 @@ Ref<Skin> Skeleton3D::create_skin_from_rest_transforms() {
 
 	// Pose changed, rebuild cache of inverses.
 	const Bone *bonesptr = bones.ptr();
-	int len = bones.size();
+	uint32_t len = bones.size();
 
 	// Calculate global rests and invert them.
 	LocalVector<int> bones_to_process;
@@ -877,7 +994,7 @@ Ref<Skin> Skeleton3D::create_skin_from_rest_transforms() {
 		}
 	}
 
-	for (int i = 0; i < len; i++) {
+	for (uint32_t i = 0; i < len; i++) {
 		// The inverse is what is actually required.
 		skin->set_bind_bone(i, i);
 		skin->set_bind_pose(i, skin->get_bind_pose(i).affine_inverse());
@@ -937,15 +1054,15 @@ void Skeleton3D::force_update_bone_children_transforms(int p_bone_idx) {
 	const int bone_size = bones.size();
 	ERR_FAIL_INDEX(p_bone_idx, bone_size);
 
-	Bone *bonesptr = bones.ptrw();
-	thread_local LocalVector<int> bones_to_process;
-	bones_to_process.clear();
-	bones_to_process.push_back(p_bone_idx);
+	Bone *bonesptr = bones.ptr();
 
-	uint32_t index = 0;
-	while (index < bones_to_process.size()) {
-		int current_bone_idx = bones_to_process[index];
+	// Loop through nested set.
+	for (int offset = 0; offset < bone_size; offset++) {
+		if (!bone_global_pose_dirty[offset]) {
+			continue;
+		}
 
+		int current_bone_idx = nested_set_offset_to_bone_index[offset];
 		Bone &b = bonesptr[current_bone_idx];
 		bool bone_enabled = b.enabled && !show_rest_only;
 
@@ -992,13 +1109,7 @@ void Skeleton3D::force_update_bone_children_transforms(int p_bone_idx) {
 		}
 #endif // _DISABLE_DEPRECATED
 
-		// Add the bone's children to the list of bones to be processed.
-		int child_bone_size = b.child_bones.size();
-		for (int i = 0; i < child_bone_size; i++) {
-			bones_to_process.push_back(b.child_bones[i]);
-		}
-
-		index++;
+		bone_global_pose_dirty[offset] = false;
 	}
 }
 
@@ -1171,20 +1282,22 @@ void Skeleton3D::_bind_methods() {
 
 #ifndef DISABLE_DEPRECATED
 void Skeleton3D::clear_bones_global_pose_override() {
-	for (int i = 0; i < bones.size(); i += 1) {
-		bones.write[i].global_pose_override_amount = 0;
-		bones.write[i].global_pose_override_reset = true;
+	for (uint32_t i = 0; i < bones.size(); i += 1) {
+		bones[i].global_pose_override_amount = 0;
+		bones[i].global_pose_override_reset = true;
 	}
 	_make_dirty();
+	_make_bone_global_poses_dirty();
 }
 
 void Skeleton3D::set_bone_global_pose_override(int p_bone, const Transform3D &p_pose, real_t p_amount, bool p_persistent) {
 	const int bone_size = bones.size();
 	ERR_FAIL_INDEX(p_bone, bone_size);
-	bones.write[p_bone].global_pose_override_amount = p_amount;
-	bones.write[p_bone].global_pose_override = p_pose;
-	bones.write[p_bone].global_pose_override_reset = !p_persistent;
+	bones[p_bone].global_pose_override_amount = p_amount;
+	bones[p_bone].global_pose_override = p_pose;
+	bones[p_bone].global_pose_override_reset = !p_persistent;
 	_make_dirty();
+	_make_bone_global_pose_subtree_dirty(p_bone);
 }
 
 Transform3D Skeleton3D::get_bone_global_pose_override(int p_bone) const {