Fixed issue where the first bone in the chain for both FABRIK and Jig…

…gle were not moving. Bone directions are now cached in the bone itself for performance reasons. Root bones now correctly report their forward and perpendicular directions if they have child bones. Still need to find a way to calculate the forward direction for isolated, childless bones though

scene/3d/skeleton_3d.cpp

@@ -389,14 +389,43 @@ float Skeleton3D::get_bone_length(int p_bone) {
 	return bones[p_bone].rest.origin.length();
 }
 
-Vector3 Skeleton3D::get_bone_axis_forward(int p_bone) {
+Vector3 Skeleton3D::get_bone_axis_forward(int p_bone, bool force_update) {
 	ERR_FAIL_INDEX_V(p_bone, bones.size(), Vector3());
-	return bones[p_bone].rest.origin.normalized();
+
+	if (bones[p_bone].rest_direction_forward.length_squared() > 0 && force_update == false) {
+		return bones[p_bone].rest_direction_forward;
+	}
+
+	// If it is a child/leaf bone...
+	if (get_bone_parent(p_bone) > 0) {
+		bones.write[p_bone].rest_direction_forward = bones[p_bone].rest.origin.normalized();
+	} else {
+		// If it has children...
+		Vector<int> child_bones = get_bone_children(p_bone);
+		if (child_bones.size() > 0) {
+			Vector3 combined_child_dir = Vector3(0, 0, 0);
+			for (int i = 0; i < child_bones.size(); i++) {
+				combined_child_dir += bones[child_bones[i]].rest.origin.normalized();
+			}
+			combined_child_dir = combined_child_dir / child_bones.size();
+			bones.write[p_bone].rest_direction_forward = combined_child_dir.normalized();
+		} else {
+			// TODO: see if there is a better way to calculate bone direction!
+			WARN_PRINT("Cannot calculate forward direction for bone " + itos(p_bone));
+			WARN_PRINT("Assuming direction of (0, 1, 0) for bone");
+			bones.write[p_bone].rest_direction_forward = Vector3(0, 1, 0);
+		}
+	}
+	return bones[p_bone].rest_direction_forward;
 }
 
-Vector3 Skeleton3D::get_bone_axis_perpendicular(int p_bone) {
+Vector3 Skeleton3D::get_bone_axis_perpendicular(int p_bone, bool force_update) {
 	ERR_FAIL_INDEX_V(p_bone, bones.size(), Vector3());
 
+	if (bones[p_bone].rest_direction_perpendicular.length_squared() > 0 && force_update == false) {
+		return bones[p_bone].rest_direction_perpendicular;
+	}
+
 	Vector3 dir = get_bone_axis_forward(p_bone);
 	dir = dir.abs();
 	if (dir.x < dir.y && dir.x < dir.z) {
@@ -406,7 +435,8 @@ Vector3 Skeleton3D::get_bone_axis_perpendicular(int p_bone) {
 	} else {
 		dir = dir.rotated(Vector3(0, 0, 1), -M_PI_2);
 	}
-	return dir.normalized();
+	bones.write[p_bone].rest_direction_perpendicular = dir.normalized();
+	return bones[p_bone].rest_direction_perpendicular;
 }
 
 // skeleton creation api

scene/3d/skeleton_3d.h

@@ -105,6 +105,11 @@ class Skeleton3D : public Node3D {
 		List<ObjectID> nodes_bound;
 		Vector<int> child_bones;
 
+		// The forward and perpendicular direction vectors are cached because they do not change
+		// 99% of the time, but recalculating them can be expensive on models with many bones.
+		Vector3 rest_direction_forward;
+		Vector3 rest_direction_perpendicular;
+
 		Bone() {
 			parent = -1;
 			enabled = true;
@@ -119,6 +124,9 @@ class Skeleton3D : public Node3D {
 			local_pose_override_amount = 0;
 			local_pose_override_reset = false;
 			child_bones = Vector<int>();
+
+			rest_direction_forward = Vector3(0, 0, 0);
+			rest_direction_perpendicular = Vector3(0, 0, 0);
 		}
 	};
 
@@ -231,8 +239,8 @@ class Skeleton3D : public Node3D {
 	// relative to the parent bone. I have not fully tested this though, but it works okay in the lookat solver.
 	// May need to be renamed in the future!
 	float get_bone_length(int p_bone);
-	Vector3 get_bone_axis_forward(int p_bone);
-	Vector3 get_bone_axis_perpendicular(int p_bone);
+	Vector3 get_bone_axis_forward(int p_bone, bool force_update = false);
+	Vector3 get_bone_axis_perpendicular(int p_bone, bool force_update = false);
 
 	// Helper functions
 	Transform global_pose_to_world_transform(Transform p_global_pose);