Refactor CCDIK3D joint processing by removing unused rotation calcula…

…tions

scene/3d/ccd_ik_3d.cpp

@@ -57,7 +57,6 @@ void CCDIK3D::_process_joints(double p_delta, Skeleton3D *p_skeleton, Vector<Man
 	int iteration_count = 0;
 	Vector<SolverInfoData> solver_info_data;
 	solver_info_data.resize(p_joints.size());
-	Quaternion destination_rotation = p_space.basis.get_rotation_quaternion() * p_skeleton->get_bone_global_pose(p_joints[p_joints.size() - 1]->bone).basis.get_rotation_quaternion();
 	for (int joints_i = 0; joints_i < p_joints.size(); ++joints_i) {
 		if (p_joints[joints_i] == nullptr) {
 			continue;
@@ -67,18 +66,8 @@ void CCDIK3D::_process_joints(double p_delta, Skeleton3D *p_skeleton, Vector<Man
 			continue;
 		}
 		SolverInfoData &current_solver_data = solver_info_data.write[joints_i];
-		const int TAIL = joints_i + 1;
-		Vector3 target = (TAIL < p_joints.size()) ? p_chain[TAIL] : p_destination;
-		Vector3 to_vec = target - p_chain[joints_i];
-		Vector3 head_to_tail = (TAIL < p_joints.size()) ? p_chain[TAIL] - p_chain[joints_i] : to_vec;
-		Quaternion to_rot;
-		if (!head_to_tail.is_zero_approx() && !to_vec.is_zero_approx()) {
-			to_rot = Quaternion(head_to_tail.normalized(), to_vec.normalized()).normalized();
-		}
 		current_solver_data.target_transform.origin = p_chain[joints_i] + p_destination;
-		current_solver_data.target_transform.basis = destination_rotation * to_rot;
 		current_solver_data.moved_transform.origin = p_chain[joints_i];
-		current_solver_data.moved_transform.basis = to_rot;
 	}
 
 	while (iteration_count < p_max_iterations) {
@@ -95,27 +84,6 @@ void CCDIK3D::_process_joints(double p_delta, Skeleton3D *p_skeleton, Vector<Man
 			}
 			bool translate = joints_i == 0;
 			Transform3D result = _solve(targets, moveds, translate);
-			Transform3D rest = p_space * p_skeleton->get_bone_global_pose(p_joints[joints_i]->bone) * Basis(p_joints[joints_i]->constraint_rotation_offset);
-			Quaternion rest_rotation = rest.basis.get_rotation_quaternion();
-			bool rotation_modified = false;
-			TwistSwing twist_swing = decompose_rotation_to_twist_and_swing(rest_rotation, result.basis.get_rotation_quaternion());
-			if (p_joints[joints_i]->twist_limitation < Math_PI) {
-				// Limit twist if needed.
-				// TODO: This is not the best way to limit the twist.
-				rotation_modified = true;
-			}
-			Ref<IKConstraint3D> constraint = p_joints[joints_i]->constraint;
-			if (constraint.is_valid()) {
-				twist_swing.swing = constraint->solve(rest_rotation, twist_swing.swing);
-				rotation_modified = true;
-			}
-			Basis recomposed_rotation;
-			if (rotation_modified) {
-				recomposed_rotation = compose_rotation_from_twist_and_swing(rest_rotation, twist_swing);
-			} else {
-				recomposed_rotation = result.basis;
-			}
-			solver_info_data.write[joints_i].moved_transform.basis = recomposed_rotation;
 			solver_info_data.write[joints_i].moved_transform.origin = result.origin;
 		}
 	}
@@ -128,8 +96,6 @@ void CCDIK3D::_process_joints(double p_delta, Skeleton3D *p_skeleton, Vector<Man
 			continue;
 		}
 		p_chain.write[joints_i] = solver_info_data[joints_i].moved_transform.origin;
-		solver_info->current_rot = solver_info_data[joints_i].moved_transform.basis.get_rotation_quaternion();
-		p_skeleton->set_bone_pose_rotation(p_joints[joints_i]->bone, solver_info_data[joints_i].moved_transform.basis);
 		bool translate = joints_i == 0;
 		if (translate) {
 			p_skeleton->set_bone_pose_position(p_joints[joints_i]->bone, solver_info_data[joints_i].moved_transform.origin);