Fixed SkeletonIK not rotating correctly when solving

scene/3d/skeleton_ik_3d.cpp

@@ -291,14 +291,10 @@ void FabrikInverseKinematic::solve(Task *p_task, real_t blending_delta, bool ove
 		new_bone_pose.origin = ci->current_pos;
 
 		if (!ci->children.is_empty()) {
-			/// Rotate basis
-			const Vector3 initial_ori((ci->children[0].initial_transform.origin - ci->initial_transform.origin).normalized());
-			const Vector3 rot_axis(initial_ori.cross(ci->current_ori).normalized());
-
-			if (rot_axis[0] != 0 && rot_axis[1] != 0 && rot_axis[2] != 0) {
-				const real_t rot_angle(Math::acos(CLAMP(initial_ori.dot(ci->current_ori), -1, 1)));
-				new_bone_pose.basis.rotate(rot_axis, rot_angle);
-			}
+			p_task->skeleton->update_bone_rest_forward_vector(ci->bone);
+			Vector3 forward_vector = p_task->skeleton->get_bone_axis_forward_vector(ci->bone);
+			// Rotate the bone towards the next bone in the chain:
+			new_bone_pose.basis.rotate_to_align(forward_vector, new_bone_pose.origin.direction_to(ci->children[0].current_pos));
 
 		} else {
 			// Set target orientation to tip