1 files changed, 110 insertions, 31 deletions

diff --git a/scene/3d/skeleton.cpp b/scene/3d/skeleton.cpp
index 8d91b6f09f..c796e47f25 100644
--- a/scene/3d/skeleton.cpp
+++ b/scene/3d/skeleton.cpp
@@ -66,7 +66,7 @@ bool Skeleton::_set(const StringName &p_path, const Variant &p_value) {
 		Array children = p_value;
 
 		if (is_inside_tree()) {
-			bones[which].nodes_bound.clear();
+			bones.write[which].nodes_bound.clear();
 
 			for (int i = 0; i < children.size(); i++) {
 
@@ -131,7 +131,7 @@ void Skeleton::_get_property_list(List<PropertyInfo> *p_list) const {
 
 		String prep = "bones/" + itos(i) + "/";
 		p_list->push_back(PropertyInfo(Variant::STRING, prep + "name"));
-		p_list->push_back(PropertyInfo(Variant::INT, prep + "parent", PROPERTY_HINT_RANGE, "-1," + itos(i - 1) + ",1"));
+		p_list->push_back(PropertyInfo(Variant::INT, prep + "parent", PROPERTY_HINT_RANGE, "-1," + itos(bones.size() - 1) + ",1"));
 		p_list->push_back(PropertyInfo(Variant::TRANSFORM, prep + "rest"));
 		p_list->push_back(PropertyInfo(Variant::BOOL, prep + "enabled"));
 		p_list->push_back(PropertyInfo(Variant::TRANSFORM, prep + "pose", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_EDITOR));
@@ -139,6 +139,59 @@ void Skeleton::_get_property_list(List<PropertyInfo> *p_list) const {
 	}
 }
 
+void Skeleton::_update_process_order() {
+
+	if (!process_order_dirty)
+		return;
+
+	Bone *bonesptr = bones.ptrw();
+	int len = bones.size();
+
+	process_order.resize(len);
+	int *order = process_order.ptrw();
+	for (int i = 0; i < len; i++) {
+
+		if (bonesptr[i].parent >= len) {
+			//validate this just in case
+			ERR_PRINTS("Bone " + itos(i) + " has invalid parent: " + itos(bonesptr[i].parent));
+			bonesptr[i].parent = -1;
+		}
+		order[i] = i;
+		bonesptr[i].sort_index = i;
+	}
+	//now check process order
+	int pass_count = 0;
+	while (pass_count < len * len) {
+		//using bubblesort because of simplicity, it wont run every frame though.
+		//bublesort worst case is O(n^2), and this may be an infinite loop if cyclic
+		bool swapped = false;
+		for (int i = 0; i < len; i++) {
+			int parent_idx = bonesptr[order[i]].parent;
+			if (parent_idx < 0)
+				continue; //do nothing because it has no parent
+			//swap indices
+			int parent_order = bonesptr[parent_idx].sort_index;
+			if (parent_order > i) {
+				bonesptr[order[i]].sort_index = parent_order;
+				bonesptr[parent_idx].sort_index = i;
+				//swap order
+				SWAP(order[i], order[parent_order]);
+				swapped = true;
+			}
+		}
+
+		if (!swapped)
+			break;
+		pass_count++;
+	}
+
+	if (pass_count == len * len) {
+		ERR_PRINT("Skeleton parenthood graph is cyclic");
+	}
+
+	process_order_dirty = false;
+}
+
 void Skeleton::_notification(int p_what) {
 
 	switch (p_what) {
@@ -176,24 +229,28 @@ void Skeleton::_notification(int p_what) {
 		case NOTIFICATION_UPDATE_SKELETON: {
 
 			VisualServer *vs = VisualServer::get_singleton();
-			Bone *bonesptr = &bones[0];
+			Bone *bonesptr = bones.ptrw();
 			int len = bones.size();
 
 			vs->skeleton_allocate(skeleton, len); // if same size, nothin really happens
 
+			_update_process_order();
+
+			const int *order = process_order.ptr();
+
 			// pose changed, rebuild cache of inverses
 			if (rest_global_inverse_dirty) {
 
 				// calculate global rests and invert them
 				for (int i = 0; i < len; i++) {
-					Bone &b = bonesptr[i];
+					Bone &b = bonesptr[order[i]];
 					if (b.parent >= 0)
 						b.rest_global_inverse = bonesptr[b.parent].rest_global_inverse * b.rest;
 					else
 						b.rest_global_inverse = b.rest;
 				}
 				for (int i = 0; i < len; i++) {
-					Bone &b = bonesptr[i];
+					Bone &b = bonesptr[order[i]];
 					b.rest_global_inverse.affine_invert();
 				}
 
@@ -205,7 +262,7 @@ void Skeleton::_notification(int p_what) {
 
 			for (int i = 0; i < len; i++) {
 
-				Bone &b = bonesptr[i];
+				Bone &b = bonesptr[order[i]];
 
 				if (b.disable_rest) {
 					if (b.enabled) {
@@ -319,12 +376,13 @@ void Skeleton::add_bone(const String &p_name) {
 
 	for (int i = 0; i < bones.size(); i++) {
 
-		ERR_FAIL_COND(bones[i].name == "p_name");
+		ERR_FAIL_COND(bones[i].name == p_name);
 	}
 
 	Bone b;
 	b.name = p_name;
 	bones.push_back(b);
+	process_order_dirty = true;
 
 	rest_global_inverse_dirty = true;
 	_make_dirty();
@@ -368,10 +426,11 @@ int Skeleton::get_bone_count() const {
 void Skeleton::set_bone_parent(int p_bone, int p_parent) {
 
 	ERR_FAIL_INDEX(p_bone, bones.size());
-	ERR_FAIL_COND(p_parent != -1 && (p_parent < 0 || p_parent >= p_bone));
+	ERR_FAIL_COND(p_parent != -1 && (p_parent < 0));
 
-	bones[p_bone].parent = p_parent;
+	bones.write[p_bone].parent = p_parent;
 	rest_global_inverse_dirty = true;
+	process_order_dirty = true;
 	_make_dirty();
 }
 
@@ -379,21 +438,24 @@ void Skeleton::unparent_bone_and_rest(int p_bone) {
 
 	ERR_FAIL_INDEX(p_bone, bones.size());
 
+	_update_process_order();
+
 	int parent = bones[p_bone].parent;
 	while (parent >= 0) {
-		bones[p_bone].rest = bones[parent].rest * bones[p_bone].rest;
+		bones.write[p_bone].rest = bones[parent].rest * bones[p_bone].rest;
 		parent = bones[parent].parent;
 	}
 
-	bones[p_bone].parent = -1;
-	bones[p_bone].rest_global_inverse = bones[p_bone].rest.affine_inverse(); //same thing
+	bones.write[p_bone].parent = -1;
+	bones.write[p_bone].rest_global_inverse = bones[p_bone].rest.affine_inverse(); //same thing
+	process_order_dirty = true;
 
 	_make_dirty();
 }
 
 void Skeleton::set_bone_ignore_animation(int p_bone, bool p_ignore) {
 	ERR_FAIL_INDEX(p_bone, bones.size());
-	bones[p_bone].ignore_animation = p_ignore;
+	bones.write[p_bone].ignore_animation = p_ignore;
 }
 
 bool Skeleton::is_bone_ignore_animation(int p_bone) const {
@@ -405,7 +467,7 @@ bool Skeleton::is_bone_ignore_animation(int p_bone) const {
 void Skeleton::set_bone_disable_rest(int p_bone, bool p_disable) {
 
 	ERR_FAIL_INDEX(p_bone, bones.size());
-	bones[p_bone].disable_rest = p_disable;
+	bones.write[p_bone].disable_rest = p_disable;
 }
 
 bool Skeleton::is_bone_rest_disabled(int p_bone) const {
@@ -425,7 +487,7 @@ void Skeleton::set_bone_rest(int p_bone, const Transform &p_rest) {
 
 	ERR_FAIL_INDEX(p_bone, bones.size());
 
-	bones[p_bone].rest = p_rest;
+	bones.write[p_bone].rest = p_rest;
 	rest_global_inverse_dirty = true;
 	_make_dirty();
 }
@@ -440,7 +502,7 @@ void Skeleton::set_bone_enabled(int p_bone, bool p_enabled) {
 
 	ERR_FAIL_INDEX(p_bone, bones.size());
 
-	bones[p_bone].enabled = p_enabled;
+	bones.write[p_bone].enabled = p_enabled;
 	rest_global_inverse_dirty = true;
 	_make_dirty();
 }
@@ -457,13 +519,13 @@ void Skeleton::bind_child_node_to_bone(int p_bone, Node *p_node) {
 
 	uint32_t id = p_node->get_instance_id();
 
-	for (List<uint32_t>::Element *E = bones[p_bone].nodes_bound.front(); E; E = E->next()) {
+	for (const List<uint32_t>::Element *E = bones[p_bone].nodes_bound.front(); E; E = E->next()) {
 
 		if (E->get() == id)
 			return; // already here
 	}
 
-	bones[p_bone].nodes_bound.push_back(id);
+	bones.write[p_bone].nodes_bound.push_back(id);
 }
 void Skeleton::unbind_child_node_from_bone(int p_bone, Node *p_node) {
 
@@ -471,7 +533,7 @@ void Skeleton::unbind_child_node_from_bone(int p_bone, Node *p_node) {
 	ERR_FAIL_INDEX(p_bone, bones.size());
 
 	uint32_t id = p_node->get_instance_id();
-	bones[p_bone].nodes_bound.erase(id);
+	bones.write[p_bone].nodes_bound.erase(id);
 }
 void Skeleton::get_bound_child_nodes_to_bone(int p_bone, List<Node *> *p_bound) const {
 
@@ -489,6 +551,8 @@ void Skeleton::clear_bones() {
 
 	bones.clear();
 	rest_global_inverse_dirty = true;
+	process_order_dirty = true;
+
 	_make_dirty();
 }
 
@@ -499,7 +563,7 @@ void Skeleton::set_bone_pose(int p_bone, const Transform &p_pose) {
 	ERR_FAIL_INDEX(p_bone, bones.size());
 	ERR_FAIL_COND(!is_inside_tree());
 
-	bones[p_bone].pose = p_pose;
+	bones.write[p_bone].pose = p_pose;
 	_make_dirty();
 }
 Transform Skeleton::get_bone_pose(int p_bone) const {
@@ -513,8 +577,8 @@ void Skeleton::set_bone_custom_pose(int p_bone, const Transform &p_custom_pose)
 	ERR_FAIL_INDEX(p_bone, bones.size());
 	//ERR_FAIL_COND( !is_inside_scene() );
 
-	bones[p_bone].custom_pose_enable = (p_custom_pose != Transform());
-	bones[p_bone].custom_pose = p_custom_pose;
+	bones.write[p_bone].custom_pose_enable = (p_custom_pose != Transform());
+	bones.write[p_bone].custom_pose = p_custom_pose;
 
 	_make_dirty();
 }
@@ -538,12 +602,21 @@ void Skeleton::_make_dirty() {
 	dirty = true;
 }
 
+int Skeleton::get_process_order(int p_idx) {
+	ERR_FAIL_INDEX_V(p_idx, bones.size(), -1);
+	_update_process_order();
+	return process_order[p_idx];
+}
+
 void Skeleton::localize_rests() {
 
-	for (int i = bones.size() - 1; i >= 0; i--) {
+	_update_process_order();
 
-		if (bones[i].parent >= 0)
-			set_bone_rest(i, bones[bones[i].parent].rest.affine_inverse() * bones[i].rest);
+	for (int i = bones.size() - 1; i >= 0; i--) {
+		int idx = process_order[i];
+		if (bones[idx].parent >= 0) {
+			set_bone_rest(idx, bones[bones[idx].parent].rest.affine_inverse() * bones[idx].rest);
+		}
 	}
 }
 
@@ -553,14 +626,14 @@ void Skeleton::bind_physical_bone_to_bone(int p_bone, PhysicalBone *p_physical_b
 	ERR_FAIL_INDEX(p_bone, bones.size());
 	ERR_FAIL_COND(bones[p_bone].physical_bone);
 	ERR_FAIL_COND(!p_physical_bone);
-	bones[p_bone].physical_bone = p_physical_bone;
+	bones.write[p_bone].physical_bone = p_physical_bone;
 
 	_rebuild_physical_bones_cache();
 }
 
 void Skeleton::unbind_physical_bone_from_bone(int p_bone) {
 	ERR_FAIL_INDEX(p_bone, bones.size());
-	bones[p_bone].physical_bone = NULL;
+	bones.write[p_bone].physical_bone = NULL;
 
 	_rebuild_physical_bones_cache();
 }
@@ -600,9 +673,12 @@ PhysicalBone *Skeleton::_get_physical_bone_parent(int p_bone) {
 void Skeleton::_rebuild_physical_bones_cache() {
 	const int b_size = bones.size();
 	for (int i = 0; i < b_size; ++i) {
-		bones[i].cache_parent_physical_bone = _get_physical_bone_parent(i);
-		if (bones[i].physical_bone)
-			bones[i].physical_bone->_on_bone_parent_changed();
+		PhysicalBone *parent_pb = _get_physical_bone_parent(i);
+		if (parent_pb != bones[i].physical_bone) {
+			bones.write[i].cache_parent_physical_bone = parent_pb;
+			if (bones[i].physical_bone)
+				bones[i].physical_bone->_on_bone_parent_changed();
+		}
 	}
 }
 
@@ -660,7 +736,7 @@ void Skeleton::physical_bones_start_simulation_on(const Array &p_bones) {
 			if (Variant::STRING == p_bones.get(i).get_type()) {
 				int bone_id = find_bone(p_bones.get(i));
 				if (bone_id != -1)
-					sim_bones[c++] = bone_id;
+					sim_bones.write[c++] = bone_id;
 			}
 		}
 		sim_bones.resize(c);
@@ -740,6 +816,8 @@ void Skeleton::_bind_methods() {
 
 #endif // _3D_DISABLED
 
+	ClassDB::bind_method(D_METHOD("set_bone_ignore_animation", "bone", "ignore"), &Skeleton::set_bone_ignore_animation);
+
 	BIND_CONSTANT(NOTIFICATION_UPDATE_SKELETON);
 }
 
@@ -747,6 +825,7 @@ Skeleton::Skeleton() {
 
 	rest_global_inverse_dirty = true;
 	dirty = false;
+	process_order_dirty = true;
 	skeleton = VisualServer::get_singleton()->skeleton_create();
 	set_notify_transform(true);
 }